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Abstract 

This  research  effort  explores  and  develops  a  real-time  sonar-based  robot  mapping  and  local¬ 
ization  algorithm  that  provides  pose  correction  within  the  context  of  a  singe  room,  to  be  combined 
with  pre-existing  global  localization  techniques,  and  thus  produce  a  single,  well-formed  map  of 
an  unknown  environment.  Our  algorithm  implements  an  expectation  maximization  algorithm 
that  is  based  on  the  notion  of  the  alpha-beta  functions  of  a  Hidden  Markov  Model.  It  performs 
a  forward  alpha  calculation  as  an  integral  component  of  the  occupancy  grid  mapping  procedure 
using  local  maps  in  place  of  a  single  global  map,  and  a  backward  beta  calculation  that  considers 
the  prior  local  map,  a  limited  step  that  enables  real-time  processing. 

Real-time  localization  is  an  extremely  difficult  task  that  continues  to  be  the  focus  of  much 
research  in  the  field,  and  most  advances  in  localization  have  been  achieved  in  an  off-line  context. 
The  results  of  our  research  into  and  implementation  of  real-time  localization  showed  limited 
success,  generating  improved  maps  in  a  number  of  cases,  but  not  all — a  trade-off  between  real¬ 
time  and  off-line  processing.  However,  we  believe  there  is  ample  room  for  extension  to  our 
approach  that  promises  a  more  consistently  successful  real-time  localization  algorithm. 
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ROBOT  MAPPING  WITH  REAL-TIME  INCREMENTAL  LOCALIZATION 
USING  EXPECTATION  MAXIMIZATION 

I.  Introduction 

Robotic  mapping  of  physical  environments  using  mobile  robots  and  their  sensors  is  consid¬ 
ered  one  of  the  most  important  steps  toward  achieving  truly  autonomous  robots.  There  exist  fairly 
robust  methods  for  mapping,  but  they  are  all  subject  to  constraints  that  do  not  always  apply  in  the 
real  world  [44],  One  key  aspect  of  robotic  mapping  is  that  it  has  two  components:  one,  sensing  the 
environment  and  converting  that  into  an  internal  representation,  and  two,  correcting  for  the  drift 
that  occurs  over  the  course  of  its  travel,  illustrated  in  Figure  1.1.  This  second  component  is  one  of 
the  most  difficult  aspects  of  robot  mapping,  especially  when  attempted  in  a  real-time  context. 

This  research  focuses  on  the  particular  task  of  performing  real-time  localization,  exploring 
and  extending  techniques  that  are  based  on  recent  research  in  the  field  of  robotic  mapping. 

1.1  Rationale 

Deeply  buried,  hardened  facilities  have  become  a  promising  and  considerably  effective 
defense  for  rogue  states  and  terrorist  organizations  against  US  firepower  that  has  otherwise  proven 
itself  against  surface  forces  and  facilities,  and  even  some  buried  targets,  with  extreme  precision 
and  lethality.  These  deeply  buried  facilities  pose  numerous  challenges  to  the  US  military,  foremost 
of  which  may  be  the  ability  to  determine  their  layout  and  contents,  intelligence  that  is  critical  to 
effective  targeting  and  weapon  system  configuration.  Robot  technology  can  assist  in  this  effort. 

This  work  envisions  the  scenario  where  sonar-guided  robots  are  deployed  through  air  shafts 
or  turmels  into  possibly  unmapped,  yet  structured,  deeply  buried  facilities  (i.e.,  with  walls,  cor¬ 
ridors,  and  rooms)  wherein  they  conduct  real-time  mapping,  localization,  and  exploration.  This 
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Figure  1.1:  Map  demonstrating  localization  error 

information  may  then  either  be  communicated  back  to  intelligence  sources  or  utilized  for  the 
delivery  of  certain  payloads. 


1.2  Problem  Statement 

The  field  of  robotic  mapping  has  matured  to  a  point  where  detailed  maps  of  complex  envi- 
romnents  can  be  built  in  real-time,  specifically  indoors.  Many  existing  techniques  are  robust  to 
noise  and  can  cope  with  a  large  range  of  environments.  However,  the  ultimate  goal  of  mapping 
is  to  solve  the  real-time  localization,  or  correspondence,  problem,  which  is  quite  challenging  and 
effectively  unsolved  even  for  static  environments.  The  goal  of  this  work  is  to  tackle  this  very 
problem — to  implement  a  robot  mapping  technique  that  uses  a  form  of  the  Bayesian  evidential 
method  [33]  for  fusing  sensor  readings  into  a  probabilistic  representation  of  the  environment,  and 
to  S5mthesize  a  variety  of  localization  techniques  that  are  currently  used  in  the  field. 


1.3  Approach 

The  means  by  which  we  address  this  problem  involves  the  building  and  representation  of 
environments  using  a  collection  of  independent  Bayesian  occupancy  grids  that  together  model 
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the  environment  using  sonar  range  data.  These  individual  grids,  called  local  maps,  cover  a  small 
area  of  the  full  environmenf,  or  global  map,  and  are  a  key  elemenf  fo  our  localization  approach. 
The  expecfed  pose  (location  and  orienfafion  relative  fo  fhe  global  environmenf)  of  each  map  is 
calculafed  and  maximized  using  a  procedure  fhaf  is  akin  fo  fhe  alpha  and  befa  functions  of  fhe 
Hidden  Markov  Model  [36]  in  fhaf  fhey  are  compufed  separafely,  one  forward  as  parf  of  fhe 
mapping  procedure  and  fhe  second  backward  over  fhose  local  maps  previously  acquired.  Local 
maps  are  reconsfrucfed  using  fhe  maximized  pose,  and  fhe  global  map  aufomafically  correcfed  as 
a  resulf. 

The  fheory  upon  which  our  work  is  based  is  foundational  fo  off-line  bafch  localizafion.  Wifh 
fhe  focus  of  our  research  being  real-fime  localizafion,  we  designed  a  localizafion  algorifhm  fhaf 
implemenfs  a  scaled  version  of  fhis  fheory,  largely  in  fhe  exfenf  over  which  fhe  befa  sfep  occurs. 
Rafher  fhan  perform  fhe  backward  calculation  over  all  previous  maps,  we  do  so  only  over  fhe 
previous  one.  Furfhermore,  our  forward  alpha  calculation  is  one  fhaf  is  implicif  in  fhe  recursive 
nafure  of  fhe  occupancy  grid  map,  and  nof  explicif. 

1.4  Thesis  Outline 

In  Chapfer  II,  we  presenf  a  brief  history  of  robofic  mapping  and  techniques  used  specifically 
for  localizafion,  as  well  as  fhose  fhaf  are  mosf  direcfly  applicable  fo  fhis  research.  We  infroduce  fhree 
common  approaches  fo  mapping:  mefric,  topological,  and  probabilistic,  focusing  in  large  parf  on 
fhe  latter  as  fhe  dominanf  fechnique  used  in  fhe  field.  We  discuss  fhe  mapping  problem,  and  several 
challenges  fhaf  sfand  in  fhe  way  of  solving  if,  such  as  sensor  noise  and  correspondence.  The  chapfer 
fhen  focuses  on  specific  probabilistic  techniques  of  Kalman  fillers,  expecfafion  maximization,  and 
hybrids  fhaf  are  used  fo  solve  differenf  aspecfs  of  fhe  mapping  problem,  presenting  fheir  basis, 
sfrengfhs,  and  weaknesses;  and  fhen  presenfs  some  of  fhe  open  problems  fhaf  remain  fo  be  solved. 
Finally,  we  round  ouf  fhe  infroducfion  by  looking  af  prior  work  in  fhe  areas  of  concurrenf  mapping 
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and  localization  and  mapping  with  real-time  localization,  which  serve  as  an  underpinning  for  our 
research. 

In  Chapter  III,  we  detail  the  theory  behind  our  mapping  and  localization  approach,  along 
with  our  implementation  details.  In  the  context  of  mapping,  we  presenf  fhe  Murphy  sonar 
model  and  fhe  means  by  which  raw  sonar  range  readings  from  a  Pioneer  robof  are  mapped  fo  a 
Bayesian  occupancy  grid,  considering  fhree  differenf  models:  poinf  of  ref  urn,  acousfic  axis,  and 
field  of  view.  Turning  from  mapping,  we  nexf  move  info  fhe  fheory  behind  our  approach  fo 
localizafion,  presenting  fhe  dafa,  motion,  and  percepfion  models  fhaf  serve  as  our  sfafisfical  basis. 
The  layered  map  represenfafion  we  utilize  is  nexf  presenfed,  wifh  discussion  of  how  if  helps  fo 
achieve  localizafion.  Finally,  we  presenf  fhe  fheory  behind  our  expecfafion  maximizafion  approach 
as  if  applies  fo  localizafion,  and  discuss  our  specific  implemenfafion  of  fhaf  fheory. 

In  Chapfer  IV,  we  presenf  fhe  resulfs  of  our  implemenfafion  in  fhe  confexf  of  simulafed  and 
real  world  environmenfs.  We  firsf  look  af  mapping  wifhouf  localizafion,  focusing  on  fhe  many 
paramefers  fhaf  are  builf  info  our  mapping  sysfem  and  fheir  effecfs  on  our  maps.  Among  fhese, 
we  consider  fhe  fhree  sonar  models  presenfed  in  Chapfer  III  and  fhose  fhaf  specifically  affecf  fhe 
Murphy  model,  such  as  how  probabilities  are  modified  in  cerfain  regions  of  fhe  sonar  field  of  view, 
and  fhe  disabling  of  cerfain  sonars.  In  fhe  second  half  of  fhe  chapfer  we  look  af  fhe  resulfs  of  our 
localizafion  implemenfafion,  discuss  paramefers  fhaf  had  fhe  greafesf  impacf  on  fhose  resulfs,  and 
presenf  several  simulafed  and  real-world  illusf rations. 

Lasfly,  we  discuss  fufure  work  and  exfensions  in  Chapfer  V.  As  our  localizafion  resulfs  were 
nof  as  successful  as  anficipafed,  we  explore  a  number  of  means  by  which  fhey  may  be  improved, 
some  simply  and  ofhers  more  involved.  We  fhen  offer  our  concluding  remarks. 
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11.  Related  Work  of  Robotic  Mapping  and  Localization 

This  chapter  presents  previous  work  related  to  robotic  mapping  and  several  key  approaches  to 
solving  the  mapping  and  localization  problems  [44]  that  are  addressed  in  this  research. 

Research  in  robotic  mapping  has  been  an  active  pursuit  for  over  two  decades.  The  problem  of 
robofic  mapping  is  fhaf  of  modeling  physical  environmenfs  using  mobile  robofs  and  fheir  sensors, 
generally  considered  one  of  fhe  mosf  imporfanf  sfeps  toward  achieving  fruly  autonomous  robofs. 
As  of  now,  fhere  are  fairly  robusf  mefhods  for  mapping  environmenfs,  buf  fhese  mefhods  are 
all  subjecf  to  a  variefy  of  non-real-world  consfrainfs,  namely  sfafic,  sfrucfured,  and  of  limited 
size.  When  if  comes  to  unsfrucfured,  d5mamic,  or  large-scale  environmenfs,  fhe  problem  is  largely 
unsolved. 

Mosf  sfafe-of-fhe-arf  mapping  algorifhms  are  probabilisfic  in  nafure.  Some  map  in  real- 
fime  [20];  ofhers  collecf  dafa  firsf  and  fhen  build  fhe  map  off-line  [27].  The  besf  resulfs  fend  fo 
be  achieved  by  fhose  fhaf  are  processed  in  mulfiple  passes  off-line.  Some  algorifhms  address  fhe 
problem  of  correspondence  befween  dafa  poinfs  fhaf  are  acquired  af  differenf  poinfs  in  time  (fhaf 
is,  such  as  when  fhe  robof  refums  fo  an  area  of  fhe  environmenf  if  has  already  visited)  [13,37],  and 
ofhers  employ  feafure-defecfion  [3, 23],  where  objecfs  in  fhe  environmenf  musf  have  a  signafure 
fhaf  makes  fhem  uniquely  identifiable. 

2.1  Historical  Overview 

In  fhe  1980s  and  1990s,  mapping  was  largely  divided  info  two  approaches:  mefric  and 
fopological.  Mefric  maps  are  mosf  commonly  represented  as  occupancy  grids,  infroduced  by  Elfes 
and  Moravec  [19,20,32],  which  model  fhe  free  and  occupied  space  of  fhe  environmenf.  The  fine 
grained  nafure  of  fhese  grids,  used  in  many  sysfems,  provides  a  resolution  fhaf  is  helpful  toward 
solving  hard  mapping  problems  (such  as  correspondence),  alfhough  fhis  comes  af  a  compufafional 
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price.  Topological  mapping  implements  the  notion  of  connectivity  between  significant  places,  such 
as  rooms,  where  arcs  from  one  place  fo  fhe  nexf  are  annofafed  wifh  navigafion  informafion. 

Since  fhe  1990s,  probabilistic  fechniques  have  dominafed  fhe  area  of  robotic  mapping  fhaf  was 
largely  inifiafed  by  Smifh,  ef .  al.  [38,39],  who  infroduced  a  sfafisfical  framework  for  simulfaneously 
building  a  map  and  localizing  such  fhaf  fhe  robof's  pose  is  correcfed  in  response  fo  odomefry  errors. 
This  procedure  has  come  fo  be  known  as  Simulfaneous  Localization  and  Mapping  (SLAM)  [15, 18] 
and  Concurrenf  Mapping  and  Localizafion  (CML)  [26,43].  One  family  of  algorifhms  employing 
fhe  probabilisfic  fechnique  is  fhaf  of  Kalman  fillers,  which  are  used  fo  esfimafe  fhe  map  and 
fhe  robof  pose  [11, 12, 16].  These  maps  generally  describe  fhe  location  of  landmarks,  significanf 
feafures  in  fhe  environmenf,  or  large  numbers  of  raw  range  measuremenfs  [27].  A  second  family 
of  algorifhms  is  based  upon  Dempsfer's  expecfafion  maximization  algorifhm  [14, 30].  These 
algorifhms  specifically  fargef  fhe  correspondence  problem,  which  is  fhe  problem  of  defermining 
whefher  sensor  measuremenfs  recorded  af  differenf  poinfs  in  time  correspond  fo  fhe  same  physical 
enfify.  A  fhird  family  of  probabilisfic  fechniques  look  fo  identify  objecfs  in  fhe  environmenf, 
such  as  ceilings,  walls,  doors  fhaf  mighf  be  open  or  closed,  and  fumifure  and  ofher  objecfs  fhaf 
move  [4,28,29]. 

2.2  The  Mapping  Problem 

The  problem  of  robofic  mapping  is  fhaf  of  acquiring  an  accurafe  spatial  model  of  an  envi¬ 
ronmenf  using  sensors  fhaf  enable  a  robof  fo  perceive  ifs  environmenf.  Sensors  commonly  used 
include  cameras;  sonar,  laser,  infrared,  and  radar  fechnologies;  facfile  sensors,  compasses,  and 
GPS.  All  fhese  sensors  are  subjecf  fo  errors,  or  noise.  The  mofion  commands  given  during  ex- 
plorafion  serve  as  imporfanf  informafion  for  building  maps,  since  fhey  convey  informafion  abouf 
fhe  locafions  af  which  differenf  sensor  measuremenfs  were  faken.  Robof  mofion  is  also  subjecf  fo 
errors,  and  fhe  confrols  alone  are  fherefore  insufficienf  fo  defermine  a  robof's  pose. 
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A  key  challenge  in  robotic  mapping  arises  from  the  nature  of  the  measurement  noise.  Model¬ 
ing  problems,  such  as  robotic  mapping,  are  usually  relatively  easy  to  solve  if  the  noise  in  different 
measurements  is  statistically  independent.  If  this  were  the  case,  a  robot  could  simply  take  more 
and  more  measurements  to  cancel  out  the  effects  of  the  noise.  Unfortunately,  in  robotic  mapping, 
the  measurement  errors  are  statistically  dependent.  This  is  in  part  due  to  the  fact  that  reflectance  in 
the  environment  will  remain  consistent,  so  that  repeated  readings  at  one  location  will  not  change 
the  inherent  noise  in  the  reading.  Also,  errors  in  control  accumulate  over  time,  which  affects  the 
way  future  sensor  measurements  are  interpreted.  This  latter  factor  is  illustrated  in  Figure  2.1, 
which  shows  how  a  small  rotational  error  on  one  end  of  a  long  corridor  can  lead  to  many  meters 
of  error  on  the  other.  As  a  result,  whatever  a  robot  infers  about  its  environment  is  plagued  by 
systematic,  correlated  errors.  Accommodating  such  systematic  errors  is  key  to  building  maps 
successfully,  and  it  is  also  a  key  complicating  factor  in  robotic  mapping.  Many  existing  mapping 
algorithms  are  therefore  complex,  both  mathematically  and  in  their  implementation. 

The  second  complicating  aspect  of  the  robot  mapping  problem  arises  from  the  high  dimen¬ 
sionality  of  the  environments  being  mapped.  For  example,  consider  how  many  numbers  it  takes 
to  describe  an  environment  like  a  person's  home.  If  confined  to  the  description  of  major  topolog¬ 
ical  entities,  such  as  hallways,  intersections,  rooms,  and  doors,  a  few  dozen  numbers  might  be 


Figure  2.1:  Odometry  error  and  the  correspondence  problem 


7 


sufficient.  A  detailed  two-dimensional  floor  plan,  which  is  a  common  representation  of  robotic 
maps,  often  requires  thousands  of  numbers.  But  a  detailed  3D  visual  map  of  a  building  may  easily 
require  millions  of  numbers.  From  a  statistical  point  of  view,  each  such  number  is  a  dimension  of 
the  underlying  estimation  problem;  thus,  the  mapping  problem  can  be  highly  dimensional. 

A  third,  and  possibly  the  hardest  problem,  in  robotic  mapping  is  the  correspondence  problem, 
also  known  as  the  data  association  problem,  which  is  the  focus  of  this  research.  The  correspondence 
problem  is  that  of  determining  if  an  object  or  portion  of  the  environment  sensed  at  one  point  in 
time  corresponds  to  the  same  object  or  environment  sensed  later  in  time.  Figure  2.1  illustrates  this 
problem  as  well,  in  which  a  robot  attempts  to  map  a  large  cyclic  environment.  When  closing  the 
cycle,  the  robot  has  to  find  out  where  it  is  relative  to  the  map  it  has  built  so  far.  This  problem 
is  complicated  by  the  fact  that  at  the  point  of  a  cycle's  closing,  the  robot's  accumulated  pose 
error  might  be  unboundedly  large.  The  correspondence  problem  is  difficult,  since  the  number  of 
possible  h5rpotheses  can  grow  exponentially  over  time. 

Fourth,  environments  change  over  time.  Some  changes  may  be  relatively  slow,  such  as  the 
change  of  appearance  of  a  tree  across  different  seasons,  or  the  structural  changes  that  most  office 
buildings  are  subjected  to  over  time.  Others  are  faster,  such  as  the  change  of  door  status  or  the 
location  of  furniture  items.  Even  faster  may  be  the  change  of  the  location  of  other  agents  in  the 
environment,  such  as  cars  or  people.  These  d5mamic  environments  create  a  big  challenge,  since 
they  add  yet  another  way  in  which  seemingly  inconsistent  sensor  measurements  can  be  explained. 
For  example,  imagine  a  robot  facing  a  closed  door  that  previously  was  modeled  as  open.  Such 
an  observation  may  be  explained  by  two  h5rpotheses,  namely  that  the  door  status  changed,  or 
that  there  is  no  door  and  the  robot  is  not  where  it  believes  to  be.  Unfortunately,  there  are  almost 
no  mapping  algorithms  that  can  learn  meaningful  maps  of  d5mamic  environments.  Instead,  the 
predominant  paradigm  relies  on  a  static  world  assumption,  as  we  do  here,  in  which  the  robot 
is  the  only  time-variant  quantity  (and  everything  else  that  moves  is  just  noise).  Consequently, 
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most  techniques  are  only  applied  in  relatively  short  time  windows,  during  which  the  respective 
environments  are  static. 

A  fifth  and  final  challenge  arises  when  robofs  musf  choose  fheir  way  during  mapping, 
commonly  called  robof  explorafion.  Any  viable  exploration  sfrafegy  has  fo  be  able  fo  accommodafe 
contingencies  and  surprises  fhaf  mighf  arise  during  map  acquisition.  For  fhis  reason,  explorafion 
is  a  challenging  plarming  problem,  which  is  of  fen  solved  sub-opfimally  via  simple  heuristics. 
When  choosing  where  fo  move,  various  quanfifies  have  fo  be  fraded  off:  fhe  expecfed  gain  in 
map  information,  fhe  time  and  energy  if  fakes  fo  gain  fhis  information,  fhe  possible  loss  of  pose 
informafion  along  fhe  way,  and  so  on.  Furfhermore,  and  mosf  imporfanfly  for  our  research,  fhe 
underlying  map  esfimafion  fechnique  musf  be  able  fo  generafe  maps  in  real-fime — an  imporfanf 
resfricfion  fhaf  rules  ouf  many  existing  approaches. 

As  nofed  above,  fhe  liferafure  refers  fo  fhe  mapping  problem  offen  in  conjuncfion  wifh 
fhe  localization  problem,  which  is  fhe  problem  of  defermining  a  robof's  pose.  The  reason  for 
suggesting  fhaf  bofh  problems  (fhe  problem  of  estimating  where  fhings  are  in  fhe  environmenf 
and  fhe  problem  of  defermining  where  a  robof  is)  have  fo  be  solved  in  conjuncfion  has  fo  do 
wifh  fhe  facf  fhaf  bofh  fhe  robof  localizafion  and  fhe  map  are  uncerfain.  By  focusing  on  jusf  one, 
fhe  ofher  infroduces  sysfemafic  noise.  If  fhe  robof's  pose  was  known  all  along,  building  a  map 
would  be  quife  simple.  If  we  already  had  a  map  of  fhe  environmenf,  fhere  already  exisf  efficienf 
algorifhms  for  defermining  fhe  robof's  pose  af  any  poinf  in  time  [5,21].  In  combination,  however, 
fhe  problem  is  much  more  difficulf. 

Today,  nearly  all  algorifhms  for  robofic  mapping  employ  probabilistic  models  of  fhe  robof 
and  ifs  environmenf,  and  rely  on  probabilistic  inference  for  fuming  sensor  measuremenfs  info 
maps.  The  reason  for  fhe  popularify  of  probabilisfic  fechniques  sfems  from  fhe  facf  fhaf  robof 
mapping  is  characferized  by  uncerfainfy  and  sensor  noise.  Percepfual  noise  is  complex  and  nof 
frivial  fo  model.  Probabilisfic  algorifhms  approach  fhe  problem  by  explicifly  modeling  differenf 
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sources  of  noise  and  their  effects  on  the  measurements.  In  the  evolution  of  mapping  algorithms, 
probabilistic  algorithms  have  emerged  as  the  sole  winner  for  this  difficult  problem  [44], 

2.2.1  Occupancy  Grid  Maps.  The  simplest  mapping  algorithm  assumes  mapping  with 
known  poses.  One  mapping  algorithm  developed  by  Elfes  and  Moravec  in  the  mid-Eighties  [20,32], 
known  as  occupancy  grid  maps,  has  been  used  by  a  number  of  autonomous  robots,  t5rpically  in 
combination  with  one  of  the  algorithms  discussed  below,  and  is  used  in  this  research  as  well. 

The  central  problem  addressed  by  occupancy  grid  mapping  and  related  algorithms  is  the 
problem  of  generating  a  consistent  metric  map  from  noisy  or  incomplete  sensor  data.  Even  if 
the  robot  poses  are  known,  it  is  sometimes  difficult  to  say  whether  a  place  in  the  environment  is 
occupied  or  not,  due  to  ambiguities  in  the  sensor  data.  The  best-explored  applications  of  occupancy 
grid  maps  require  robots  with  range  sensors,  such  as  sonar  sensors  or  laser  range  finders.  Both 
sensors  are  characterized  by  noise.  Sonars,  in  addition,  cover  an  entire  cone  in  space,  and  from 
a  single  sonar  measurement  it  is  impossible  to  say  where  in  the  cone  the  sensed  object  is.  Both 
sensors  are  also  sensitive  to  the  angle  of  an  object  surface  relative  to  the  sensor  and  the  reflective 
properties  of  the  surface  (absorption  and  dispersion). 

Occupancy  grid  maps  address  such  problems  by  generating  probabilistic  maps.  As  the  name 
suggests,  occupancy  grid  maps  are  represented  by  grids,  which  are  usually  two-dimensional.  The 
standard  occupancy  grid  mapping  algorithm  is  a  version  of  Bayes  filters  [44].  The  occupancy  grid 
mapping  algorithm  is  recursive,  allowing  for  incrementally  updating  the  individual  grid  cells  as 
new  sensor  data  arrives. 

Einally,  calculating  occupancy  grid  maps  requires  two  probability  densities,  one  that  repre¬ 
sents  the  prior  probability  of  occupancy  and  another,  the  inverse  sonar  model,  that  specifies  the 
probability  that  a  grid  cell  is  occupied  based  on  a  single  sensor  measurement.  Inverse  models 
for  range  sensors  usually  attribute  high  probability  of  occupancy  for  grid  cells  that  overlap  with 
detected  range,  and  low  probability  for  grid  cells  in  between  this  range  and  the  sensor. 
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2.2.1.1  Histogramic  In-Motion  Mapping.  Histogramic  In-Motion  Mapping  (HIMM) 
is  part  of  a  real-time  obstacle  avoidance  system  developed  at  Carnegie  Mellon  University  that  not 
only  produces  maps,  but  also  provides  instantaneous  environmental  data  for  use  by  an  infegrafed 
obsfacle  avoidance  sysfem.  The  mefhod  represenfs  dafa  in  a  fwo-dimensional  hisfogram  grid 
(Figure  2.2),  based  on  fhe  occupancy  grid,  where  belief  of  occupancy  is  represenfed  by  discrefe 
hisfogramic  values  rafher  fhan  probabilifies.  The  sysfem  fakes  a  range  reading  from  each  sonar 
and  maps  fhaf  reading  fo  fhe  specific  cell  fhaf  corresponds  fo  fhe  measured  disfance  along  fhe 
acousfic  axis.  The  value  in  fhis  cell  is  incremenfed  by  3,  and  fhe  values  of  all  fhe  cells  preceding  if 
along  fhe  acousfic  axis  are  decremenfed  by  1;  minimum  and  maximum  values  are  capped  at  0  and 
15,  respectively.  The  primary  purpose  for  using  fhis  approach  in  lieu  of  a  probabilistic  model  is 
speed:  fhe  probabilistic  cerfainfy  grid  updafes  all  cells  wifhin  fhe  cone  of  fhe  sonar,  which  involves 
considerably  more  calculation.  [6] 


Figure  2.2:  Mapping  an  object  using  HIMM  [6] 


2.2.2  Kalman  Filters.  A  classical  approach  fo  generating  maps  is  based  on  Kalman  fillers, 
referred  fo  in  fhe  liferafure  as  Simulfaneous  Localizafion  and  Mapping  [24].  This  approach  can 
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be  traced  back  to  a  highly  influential  series  of  papers  by  Smith,  Self,  and  Cheeseman  [38, 39], 
who  in  1985  fhrough  1990  proposed  a  mafhemafical  formulation  of  fhe  approach  fhaf  is  sfill  in 
widespread  use  today  In  fhe  following  years,  a  number  of  researchers  developed  fhis  approach 
furfher  [11,16,18,26]. 

Kalman  fillers  are  Bayesian  fillers  fhaf  represenf  posferiors  P{st,  m\z‘,  u‘)  wifh  Gaussians  [44], 
which  models  fhe  probabilify  of  fhe  robof's  pose  s  af  lime  t  and  fhe  map  m  given  fhe  sensor  reading  z 
and  robof  confrol  u,  also  af  lime  t.  In  a  fwo-dimensional  case,  fhe  pose  s  is  usually  modeled  by  fhree 
variables:  fhe  Carfesian  coordinates  in  fhe  plane  and  fhe  heading  direcfion.  The  motion  model 
(Section  3.2. 1.2)  is  linear  wifh  added  Gaussian  noise,  as  is  fhe  perception  model  (Section  3.2. 1.3). 
The  map  is  represenfed  as  a  collection  of  landmark  locations,  along  wifh  Gaussian  uncerfainfy  as 
fo  fheir  locafion. 

The  primary  advanfage  of  fhe  Kalman  filler  approach  is  fhaf  if  esfimafes  fhe  full  posterior^ 
over  maps  in  an  online  fashion,  alfhough  pre-processing  and  filtering  for  landmark  defection  is 
necessary  [44].  To  date,  fhe  only  algorifhms  fhaf  are  capable  of  estimating  fhe  full  posterior  are 
based  on  Kalman  fillers  or  exfensions  fhereof,  such  as  mixfure  of  Gaussian  mefhods  [18],  or  a  Rao- 
Blackwellized  particle  filter  [17,31].  There  are  many  advanfages  fo  estimating  fhe  full  posferior. 
In  addition  fo  fhe  mosf  likely  map  and  robof  poses,  Kalman  fillers  mainfain  fhe  full  uncerfainfy 
in  fhe  map,  which  can  be  highly  beneficial  when  using  fhe  map  for  navigation.  Additionally,  fhe 
approach  can  be  shown  fo  converge  wifh  probabilify  one  fo  fhe  frue  map  and  robof  position,  up 
fo  a  residual  uncerfainfy  disfribufion  fhaf  largely  stems  from  an  initial  random  drift  [16,34].  As  is 
commonly  fhe  case  wifh  fheorefical  resulfs,  fhey  only  hold  under  specific  condifions  and  require 
fhaf  fhe  robof  encounfer  each  landmark  infinifely  often.  Even  so,  fhis  is  fhe  sfrongesf  convergence 
resulf  fhaf  presenfly  exisfs  in  fhe  confexf  of  robotic  mapping. 

^The  full  posterior  probability  is  generally  given  as  P{Hi\E)  =  '  ™here  the  probability  of  a  hypothesis 

given  the  evidence  P{H\E)  is  equal  to  the  likelihood  of  the  evidence  given  the  hypothesis  P{E\H)  multiplied  by  the  prior 
probability  of  the  hypothesis  P(H). 
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Probably  the  most  important  limitation  of  the  Kalman  filter  approach  lies  in  the  Gaussian 
noise  assumption,  which  states  that  the  measurement  noise  must  be  independent  and  Gaussian. 
This  has  important  implications  for  pracfical  implemenfafions.  For  example,  in  an  environmenf 
wifh  two  indistinguishable  landmarks,  a  multimodal  disfribufion  over  possible  robof  poses  will 
resulf,  confrary  fo  fhe  unimodal  Gaussian  noise  assumption.  As  a  resulf,  Kalman  filler  approaches 
are  unable  fo  handle  fhe  correspondence  problem,  a  significanf  limifafion  for  our  research. 

2.2.3  Expectation  Maximization  Algorithms.  An  alfernafive  fo  fhe  Kalman  filler  paradigm 
is  known  as  fhe  expecfafion  maximization  (EM)  family  of  algorifhms.  EM  is  a  sfafisfical  algorifhm 
fhaf  was  developed  in  fhe  confexf  of  maximum  likelihood  esfimafion  wifh  lafenf  variables  in  an 
influenfial  paper  by  Dempsfer,  Laird  and  Rubin  [14]. 

Applied  fo  fhe  robotic  mapping  problem,  EM  algorifhms  consfifufe  today's  besf  solutions 
fo  fhe  correspondence  problem  in  mapping  [9,43].  In  particular,  EM  algorifhms  have  been  found 
fo  generafe  consisfenf  maps  of  large-scale  cyclic  environmenfs,  even  if  all  feafures  look  alike  and 
cannof  be  distinguished  percepfually  [44].  However,  EM  algorifhms  do  nof  refain  a  full  notion 
of  uncerfainfy.  Insfead,  fhey  perform  hill  climbing  in  fhe  space  of  all  maps  in  an  affempf  fo 
find  fhe  mosf  likely  map  (nof  necessarily  fhe  frue  map).  To  do  so,  fhey  have  fo  process  fhe  dafa 
mulfiple  fimes;  fherefore,  EM  algorifhms  cannof  generafe  maps  incremenfally,  as  is  fhe  case  for 
many  Kalman  filter  approaches,  and  fhus  nof  in  real-fime,  a  requiremenf  for  fhis  research. 

Eor  fhis  approach,  maps  are  represenfed  as  poinf  obsfacles,  where  poinfs  correspond  fo 
comers,  infersecfions,  and  ofher  disfincfive  places.  These  poinfs  are  used  fo  form  a  topological  map 
represenfafion,  and  overlayed  wifh  raw  sensor  dafa  fo  build  an  occupancy  grid  map,  illusfrafed  in 
Eigure  2.3.  Existing  implemenfafions  rely  on  grid  represenfafions  for  all  densities  involved  in  fhe 
esfimafion  process,  and  also  rely  on  probabilistic  maps  insfead  of  zero-one  maps  (Section  2. 2.1.1). 
The  pose  represenfafion  is  fhe  same  as  fhaf  for  fhe  Kalman  filter  approach;  however,  fhe  sensor 
noise  does  nof  have  fhe  Gaussian  resfricfion,  buf  rafher  can  be  of  any  f5q)e. 
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Figure  2.3:  EM  point  obstacles  [44] 


The  EM  algorithm  builds  on  the  insight  that  building  a  map  when  the  robot's  path  is  known 
(in  expectation)  is  relatively  simple,  as  is  finding  a  probabilistic  estimate  of  fhe  robof's  locafion  for 
a  given  known  map.  If  does  fhis  by  iferafing  over  two  sfeps:  an  expecfafion  sfep  (E-sfep),  where 
fhe  posferior  over  robof  poses  (fhe  locations  of  possible  robof  poses)  is  calculafed  for  a  given  map, 
and  a  maximizafion  sfep  (M-sfep),  in  which  EM  calculafes  fhe  mosf  likely  map  given  fhese  pose 
expecfafions,  resulfing  in  a  series  of  increasingly  accurafe  maps.  The  pose  probabilities  calculafed 
in  fhe  E-sfep  correspond  fo  differenf  h5rpofheses  as  fo  where  fhe  robof  mighf  have  been,  and 
suggesf  a  number  of  possible  correspondences  wifh  ofher  mapped  elemenfs  in  fhe  environmenf. 
By  building  maps  in  fhe  M-sfep,  fhese  correspondences  are  franslafed  info  feafures  in  fhe  map, 
such  as  walls  or  ofher  obsfacles,  which  fhen  eifher  gef  reinforced  in  fhe  nexf  E-sfep  or  gradually 
disappear. 

Advanfages  of  fhe  EM  algorifhm  over  Kalman  tillering  include  ifs  abilify  fo  build  maps  from 
raw  sensor  dafa  and  mosf  significanfly,  ifs  abilify  fo  solve  fhe  correspondence  problem  (see  Eigure 
2.4),  alfhough  only  in  an  off-line  fashion.  A  scaled  form  of  fhis  procedure  is  used  in  fhis  research. 


2.2.4  Hybrid  Approaches.  There  are  many  examples  of  hybrid  solufions  which  infegrafe 
probabilisfic  posferiors  wifh  compufafionally  more  efficienf  maximum  likelihood  esfimafes. 

One  of  fhe  mosf  common  approaches  is  fhe  incremenfal  maximum  likelihood  mefhod  [20, 
32,40].  The  basic  idea  is  fo  incremenfally  build  a  single  map  as  fhe  sensor  dafa  arrives,  buf  wifhouf 
keeping  frack  of  any  residual  uncerfainfy.  Such  a  mefhodology  can  be  viewed  as  an  M-sfep  in 
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Figure  2.4:  EM  applied  to  a  cyclic  environment,  showing  raw  range  data  of  a  large  hall  in  a  museum 

that  is  aligned  using  artificial  but  indistinguishable  landmarks  [44] 


EM,  without  an  E-step.  The  advantage  of  this  approach  lies  in  its  simplicity,  which  accounts  for  its 
popularity. 

Like  Kalman  filters,  this  approach  can  build  maps  in  real-time,  but  without  maintaining 
a  notion  of  uncertainty.  Like  EM,  it  maximizes  likelihood.  However,  the  incremental  one-step 
likelihood  maximization  differs  from  the  likelihood  maximization  over  an  entire  data  set.  In 
particular,  once  a  pose  and  a  map  have  been  determined,  they  are  frozen  once  and  forever  and 
carmot  be  revised  based  on  future  data — a  key  feature  of  both  the  Kalman  filter  and  the  EM 
approach. 

This  weakness  manifests  itself  in  the  inability  to  map  cyclic  environments,  where  the  error  in 
the  pose  may  grow  without  bounds.  Eigure  2.5  shows  an  example  where  the  incremental  maximum 
likelihood  approach  is  used  to  map  a  cyclic  enviromnent,  with  a  robot  equipped  with  a  laser  range 
finder.  While  the  map  is  reasonably  consistent  before  closing  the  loop,  the  large  residual  error 
leads  to  inconsistencies  that  cannot  be  resolved  by  the  incremental  maximum  likelihood  approach. 
This  is  a  general  limitation  of  algorithms  that  do  not  consider  uncertainty  when  building  maps, 
and  that  possess  no  mechanism  to  use  future  data  to  adjust  past  decisions. 
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Figure  2.5:  Incremental  maximum  likelihood  applied  to  map  of  cyclic  environment  [44] 


Hybrid  approaches  overcome  this  limitation  by  maintaining  an  explicit  notion  of  uncertainty 
during  mapping,  short  of  fhe  full  posferior  over  maps  and  poses  mainfained  by  Kalman  fillers. 
A  good  example  are  fhe  algorifhms  described  in  [22,41,42],  which  use  fhe  incremenfal  maximum 
likelihood  approach  fo  build  maps,  buf  in  addifion  mainfain  a  posferior  disfribufion  over  robof 
poses.  The  idea  is  fhaf  by  refaining  a  nofion  of  fhe  robof's  pose  uncerfainfy,  conflicfs  like  fhe 
one  faced  by  fhe  incremenfal  maximum  likelihood  mefhod  can  be  idenfified,  and  fhe  appropriafe 
corrective  action  can  be  faken.  Figure  2.6  shows  a  sequence  of  map  esfimation  sfeps  using  this 
approach  using  the  same  data  as  that  for  Figure  2.5.  When  fhe  robof  fraverses  a  cyclic  environmenf, 
if  uses  fhe  samples  fo  localize  if  self  relative  fo  fhe  previously  builf  map.  When  if  has  defermined 
ifs  pose  wifh  high  likelihood,  if  uniformly  spreads  fhe  resulting  error  along  fhe  cycle  in  fhe 
map.  Consequenfly,  fhe  approach  sfill  mainfains  jusf  a  single  map,  which  is  compufafionally 
advanfageous.  Buf  unlike  fhe  incremenfal  maximum  likelihood  mefhods,  if  also  has  fhe  ability 


Figure  2.6:  Hybrid  mapping  applied  to  map  of  cyclic  environment  [44] 
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to  correct  its  map  backwards  in  time  whenever  an  inconsistency  is  detected.  Mathematically,  this 
hybrid  algorithm  can  be  derived  as  a  rather  crude  approximation  to  the  EM  algorithm,  which 
performs  the  E-step  and  the  M-step  selectively,  as  discrepancies  are  detected. 

The  hybrid  approach  suffers  many  deficiencies.  Eirsf  and  foremosf,  fhe  decision  fo  change 
fhe  map  backwards  in  time  is  a  discrefe  one  which,  if  wrong,  can  lead  fo  cafasfrophic  failure. 
Moreover,  fhe  approach  carmof  cope  wifh  complex  ambiguities,  such  as  fhe  uncerfainfy  fhaf 
arises  when  fhe  robof  fraverses  multiple  nesfed  cycles.  Einally,  fhe  hybrid  approach  is — sfricfly 
speaking — nof  a  real-fime  algorifhm,  since  fhe  fime  if  fakes  fo  correcf  a  loop  depends  on  fhe  size 
of  fhe  loop.  However,  practical  implemenfafions  appear  fo  work  well  in  real-fime  when  used  in 
office-building  t5rpe  environmenfs. 

Since  our  implemenfafion  is  a  hybrid  approach  ifself,  if  shares  many  of  fhe  characferisfics 
described  here,  parficularly  in  ifs  incremenfal  nafure,  ifs  vulnerabilify  fo  cycles,  and  ifs  discrefe 
decision  fo  change  a  map  backwards  in  fime.  However,  if  differs  in  fhaf  a  local  map's  pose 
is  nof  once  defermined  and  fhereaffer  consfanf;  rafher,  if  may  be  updafed  as  a  resulf  of  fufure 
localizafions.  Also,  rafher  fhan  firsf  collecting  a  map  and  fhen  localizing  off-line,  our  approach 
maps  and  localizes  in  real  fime. 

2.2.5  Open  Problems.  Affer  nearly  fhree  decades  of  research,  fhe  field  of  robofic  mapping 
has  mafured  fo  a  poinf  where  defailed  maps  of  complex  environmenfs  can  be  builf  in  real-fime, 
specifically  indoors.  Many  exisfing  fechniques  are  robusf  fo  noise  and  can  cope  wifh  a  large  range 
of  environmenfs.  Neverfheless,  fhere  still  exisf  a  large  number  of  challenging  open  problems. 

In  particular,  mosf  published  mefhods  assume  fhaf  fhe  world  does  nof  change  during  map¬ 
ping.  Real  environmenfs  are  d5mamic,  such  as  would  be  fhe  case  for  personal  service  robofs.  A 
second  shorfcoming  of  currenf  fechnology  arises  from  fhe  vasf  amounf  of  knowledge  fhaf  we,  as 
people,  possess  abouf  environmenfs,  buf  fhaf  is  presenfly  nof  applied  fo  fhe  mapping  problem, 
due  fo  our  inabilify  fo  algorifhmically  process  voluminous  amounfs  of  dafa. 
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From  a  statistical  point  of  view,  the  ultimate  goal  of  mapping  is  fo  solve  fhe  correspondence 
problem.  While  fhis  problem  is  hard  and  effecfively  unsolved  even  for  sfafic  environmenfs,  if  is 
even  more  challenging  for  d5mamic  environmenfs. 

Anofher  dimension  worfh  exploring  is  fhe  topic  of  unsfrucfured  environmenfs.  Mosf  exam¬ 
ples  in  fhis  survey  focused  on  indoor  environmenfs,  which  possess  a  lof  of  sfrucfure.  Oufdoor, 
underwater,  and  planefary  environmenfs  also  possess  some  sfrucfure,  buf  come  in  a  much  larger 
variefy.  In  fhe  exfreme,  fhe  environmenf  may  be  entirely  unsfrucfured,  such  as  piles  of  rubble  fhaf 
a  robof  mighf  wanf  fo  explore  affer  a  nafural  disaster  or  a  ferrorisf  attack.  Many  of  fhe  fechniques 
described  here  are  inapplicable. 

2.3  Research-Specific  Literature 

Of  fhe  general  research  presenfed  fhus  far,  fhere  are  specific  endeavors  fhaf  serve  as  a  sfrong 
foundafion  for  fhis  research,  addressing  a  probabilisfic  approach  fo  real-fime  concurrenf  mapping 
and  localization. 

2.3.1  Concurrent  Mapping  and  Localization.  Thrun,  ef.  al.  [43]  address  fhe  problem  of 
building  large-scale  geomefric  maps  of  indoor  environmenfs,  posing  fhe  map  building  problem  as 
a  consfrained,  probabilisfic  maximum  likelihood  estimation  problem.  The  general  problem  fhey 
address  is  how  a  robof  can  consfrucf  a  consisfenf  map  of  fhe  environmenf  if  if  only  occasionally 
observes  a  landmark  (as  opposed  fo  frequenfly),  specifically  when  fhose  landmarks  may  be  indis¬ 
tinguishable  and  where  fhe  accumulafed  odomefric  error  may  be  large.  They  presenf  an  algorifhm 
for  generafing  fhe  mosf  likely  map  from  fhe  dafa,  along  wifh  fhe  mosf  likely  pafh  faken  by  fhe 
robof.  Their  resulfs  are  effecfive  in  cyclic  environmenfs  of  up  fo  80  x  25  mefers. 

The  paper  presenfs  an  algorifhm  for  landmark-based  map  acquisition  and  concurrenf  lo- 
calizafion  fhaf  is  based  on  a  sfafisfical  accounf  of  robof  motion  and  perception.  The  problem  of 
map  building  is  posed  as  a  maximum  likelihood  esfimafion  problem,  where  bofh  fhe  locafion  of 
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landmarks  and  the  robot's  position  have  to  be  estimated.  Likelihood  is  maximized  under  prob¬ 
abilistic  constraints  that  arise  from  the  physics  of  robof  mofion  and  perception.  The  problem  is 
solved  efficientiy  using  fhe  Baum-Welch  (or  alpha-befa)  algorifhm  [35],  which  is  a  special  version 
of  EM  [14]  in  how  if  alfemafes  fhe  E-sfep  and  M-sfep  (somefimes  also  called  modification  sfep). 
In  fhe  E-sfep,  fhe  currenf  map  is  held  consfanf  and  fhe  probabilify  disfribufions  are  calculafed  for 
pasf  and  currenf  robof  locations.  In  fhe  M-sfep,  fhe  mosf  likely  map  is  compufed  based  on  fhe 
estimation  resulf  of  fhe  E-sfep.  By  alfernafing  bofh  sfeps,  fhe  robof  simulfaneously  improves  ifs 
localizafion  and  ifs  map,  which  leads  fo  a  local  maximum  in  likelihood  space.  The  probabilisfic 
nafure  of  fhe  estimation  algorifhm  makes  if  considerably  robusf  fo  ambiguifies  and  noise,  bofh  in 
fhe  odomefry  and  in  percepfion.  If  also  enables  fhe  robof  fo  revise  pasf  location  esfimafes  as  new 
sensor  dafa  arrives. 

While  fhe  fheory  presenfed  in  fhis  work,  defailed  in  Section  3.2.1,  is  a  key  componenf  of  fhe 
research  here,  fhe  model  does  operafe  under  several  assumptions  or  resfricfions.  One,  if  assumes 
fhaf  a  robof  operafor  selecfs  a  small  number  of  significanf  places  (landmarks  such  as  infersecfions, 
comers,  or  dead  ends),  where  fhe  robof  is  informed  each  time  such  a  place  has  been  reached; 
fhaf  fhe  robof  can  observe  landmarks;  and  fhaf  if  has  fhe  means  for  esfimafing  fheir  f5rpe,  relafive 
angle,  and  approximafe  disfance.  Also,  in  fheir  model  of  robof  percepfion  (fhe  probabilify  of  an 
observafion  given  fhe  currenf  pose  and  map  of  fhe  environmenf),  fhe  map  confains  knowledge 
abouf  fhe  exacf  locafion  of  all  landmarks,  leaving  fhe  pose  alone  unknown.  Eurfhermore,  fhe 
localizafion  is  nof  conducfed  simulfaneously  wifh  fhe  mapping  in  real  time. 

In  follow-on  work,  Burgard,  ef.  al.  [10],  exfended  fhis  research  by  changing  fheir  single-map 
info  a  layered  represenfafion  of  maps,  where  a  global  map  is  composed  of  a  collecfion  of  small, 
local  maps  fhaf  are  generafed  from  shorf  sequences  of  sonar  dafa  .  This  approach  is  based  on  fhe 
nof  ion  fhaf  shorf-ferm  odomefry  errors  are  f5rpically  small  and  safely  ignored.  These  local  maps, 
annofafed  wifh  fheir  pose  disfribufions,  fake  fhe  place  of  fhe  landmarks  used  in  fhe  previous 
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approach.  This  approach  allows  for  the  maintaining  of  dependencies  between  different  grid  cells 
within  each  local  map,  which  is  in  contrast  to  the  independence  assumption  inherent  in  the  single 
global  map.  The  result  is  they  are  able  to  use  conventional  metric  Markov  localization  in  the 
E-step,  and  are  able  to  simultaneously  operate  on  raw  proximity  information  without  relying 
on  pre-defined  landmarks.  Their  empirical  results  illustrate  success  in  learning  large-scale  maps 
based  solely  on  sonar  range  data. 

While  it  is  this  notion  of  local  maps  that  we  implement  within  this  research,  and  much  of  the 
theory  upon  which  their  EM  is  based,  their  algorithm  does  remain  an  off-line  batch  localization. 
As  a  result,  our  research  implements  a  scaled  version  so  as  to  achieve  real-time  localization. 

In  additional  CML  work  to  which  our  research  is  directly  related,  Laviers  [25]  uses  an  occu¬ 
pancy  grid  to  model  individual  rooms  while  they  are  being  mapped,  and  then  upon  completion 
of  each  room,  converts  them  into  a  polygonal  representation.  These  reduced-information  environ¬ 
ment  representations  are  further  reduced  to  a  connected  graph  of  Absolute  Space  Representations 
(ASRs).  The  approach  performs  real-time  localization  on  the  ASRs  using  EM,  localizing  the  indi¬ 
vidual  rooms  to  each  other;  there  is  no  localization  occurring  within  the  rooms.  One  goal  of  our 
research  is  to  provide  this  in-room  localization. 

2.3.2  Mapping  with  Real-time  Localization.  The  mapping  and  localization  techniques 
presented  in  [45]  are  specifically  relevant  to  our  research  for  the  means  by  which  they  build  the 
occupancy  grid,  determine  a  robot's  expected  pose,  and  use  the  notion  of  local  maps  to  build  a 
single  global  map. 

While  the  localization  method  requires  as  input  a  global  map  that  identifies  occupied  cells, 
it  is  their  approach  to  determining  where  the  robot  is  in  the  global  map  that  has  application  here. 
In  brief,  a  single  sonar  range  sweep  is  taken,  itself  considered  a  single  local  map,  and  the  range 
vectors  generated  by  the  sweep  are  compared  against  all  locations  on  the  global  map.  Those  that 
are  feasible  poses,  that  is,  those  where  the  vector  is  not  blocked  by  an  obstacle  are  added  to  a 
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Figure  2.7:  Localization  using  feasible  poses  over  a  known  map  [8] 


collection  of  feasible  poses.  The  infersecfion  of  fhese  poses  identifies  fhe  location  of  fhe  robof. 
This  is  illusfrated  in  Figure  2.7:  Given  a  map  M  (a)  and  a  range  vecfor  x  (b),  fhey  defermine  fhe 
sef  of  feasible  poses  FP(M,x)  (c);  along  wifh  range  vecfor  y  (d)  and  feasible  poses  FP{M,  y)  (e),  fhe 
infersecfion  of  poses  FP{m,  {x,  y\)  (f)  narrows  fhose  feasible  down  fo  fwo;  and  finally,  range  vecfor 
z  (g)  is  added  wifh  feasible  poses  FP{m,z)  (h),  fo  yield  fhe  singular  feasible  pose  (i).  [8] 

The  resulfs  presenfed  by  Varveropoulos  are  impressive,  buf  a  key  shorfcoming  is  fhaf  prepro¬ 
cessing  using  a  global  map  is  required  [2].  And  alfhough  [8]  address  localizafion  wifhouf  explicif 
landmarks,  fhey  too  are  using  a  global  map  and  a  robof  fhat  is  manually  positioned  af  various 
locafions  wifhin  fhe  environmenf. 


2.4  Contribution  of  this  Study 

With  the  significant  progress  that  has  been  made  in  autonomous  robotic  mapping,  each 
approach  makes  several  assumptions  or  is  subject  to  various  constraints  so  as  to  break  the  larger 
problem  down  into  smaller  bits  that  may  one  day  be  recombined  into  a  far-distant  mapping 
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algorithm  that  makes  no  assumptions  and  has  no  constraints.  For  this  research,  our  goal  is  to  map 
a  static  environment  using  sonar  with  real-time  localization  in  a  local  context,  that  is,  in  the  context 
of  a  single  room,  that  is  to  be  combined  with  prior  work  in  global  localization  using  absolute  space 
representations  (ASR). 
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III.  Methodology 

In  this  chapter  we  present  the  theory  and  implementation  central  to  our  research.  Our  mapping 
algorithm  is  an  implementation  of  a  Bayesian  occupancy  grid  that  uses  a  linear  sonar  model.  We 
generate  three  different  representations  of  fhis  model  fhaf  assisf  in  producing  maps  mosf  conducive 
fo  localizafion.  Our  localizafion  is  based  upon  an  HMM-based  EM  approach  fhaf  ufilizes  local 
maps  fo  correcf  for  robof  pose  error. 

3.1  Mapping 

The  occupancy  grid  discussed  in  secfion  2.2.1  is  one  componenf  of  our  implemenfafion, 
which  is  based  on  fhe  work  of  Murphy  [33,  pp  380-386].  In  fhis  model,  an  ulfrasonic  range  reading 
is  mapped  fo  a  specific  cell  on  fhe  occupancy  grid,  and  probabilifies  of  occupancy  are  calculafed 
for  fhaf  cell  and  fhose  in  surroundings  regions. 

3.1.1  The  Sonar  Model.  Fundamenfal  fo  fhis  research  is  an  undersfanding  of  fhe  sonar 
sensor  model,  illusfrafed  in  Figure  3.1,  and  defailed  by  [33,  pp  378-380].  A  wave  emifs  from  fhe 
sonar  device  along  fhe  acousfic  axis  fhaf  spreads  and  generafes  a  field  of  view  of  approximafely 
30°,  as  defermined  by  fhe  Polaroid  sonars  on  fhe  Pioneer  hardware  we  ufilize.  Wifhin  a  cerfain 
maximum  range,  fhe  wave  will  bounce  off  an  objecf  and  refurn  fo  fhe  sonar  sensor,  af  which  poinf 
a  disfance  fo  fhaf  objecf  will  be  calculafed  based  on  fhaf  wave's  fime  of  fravel.  This  disfance  is 
measured  along  fhe  acousfic  axis,  fhe  line  bisecfing  fhe  cone;  fhe  objecf 's  angle  off  fhe  acousfic  axis 
is  nof  measurable. 

This  beam  can  be  projecfed  onto  a  grid,  as  shown  in  Figure  3.1.  This  is  commonly  called  an 
occupancy  grid  because  a  value  can  be  calculafed  for  each  cell,  represenfing  fhe  likelihood  fhaf  fhe 
cell  is  occupied. 

While  fhe  sonar  reading  is  idenfifying  an  objecf  af  fhe  infersecfion  of  fhe  acousfic  axis  and 
fhe  cenfer  of  fhe  darkened  region  in  fhe  figure,  call  if  Cc,  fhe  noisy  nafure  of  fhe  sonar  leads  fo 
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Figure  3.1:  The  probabilistic  sonar  model  over  an  occupancy  grid  [7] 


the  possibility  that  the  object  is  actually  off  the  axis  or  slightly  behind  or  in  front  of  the  region's 
center.  In  a  probabilistic  occupancy  grid,  this  is  acknowledged  by  calculating  an  increasingly  lower 
probability  of  occupancy  for  the  area  surrounding  Cc,  represented  in  Figure  3.1(a)  by  the  darkly 
shaded  (red)  domes.  As  shown,  the  distribution  is  ideally  Gaussian.  For  efficiency,  however, 
Murphy  employs  a  linear  distribution,  which  we  implement  here  (Figure  3.2). 

Murphy  details  the  sonar  model  by  identifying  some  key  nomenclature  representing  various 
measures  and  regions.  The  half-angle  of  the  sonar  beam  is  specified  by  ji,  and  the  maximum 
range  of  the  sonar  is  specified  by  R.  Additionally,  a  particular  sonar  beam  is  divided  up  into  three 
regions: 


Region  I:  the  narrow  area  around  the  sensed  object,  for  which  there  is  a  high  confidence  of 
occupancy 

Region  II:  the  area  preceding  Region  I,  for  which  there  is  a  high  confidence  of  vacancy 
Region  III:  the  area  beyond  Region  I,  for  which  no  estimation  of  occupancy  is  made 
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When  considering  the  cells  that  fall  within  either  Region  I  or  II,  another  key  component  is  the 
notion  of  a  cell's  disfance  off  fhe  acoustic  axis,  specified  by  a,  and  ifs  disfance  from  fhe  sensor, 
specified  by  r. 


Wifh  fhese  four  ferms,  and  knowledge  of  which  region  a  cell  falls  info,  a  probabilify  fhaf  a 
particular  cell  is  occupied  given  a  sonar  reading  is  calculafed.  This  probabilify  model  is  illusfrafed 
in  Figure  3.2(b),  where  fhe  closer  fo  fhe  sonar  device  a  cell  is,  fhe  higher  fhe  probabilify  if  is  eifher 
occupied  (Region  I)  or  empfy  (Region  II). 


t  .0  Occupied 


Region  II  I  III 

(b) 


Figure  3.2:  Modeling  sonar  probabilities,  showing  full  Gaussian  (a)  and  linear  (b)  represenfafions. 


The  firsf  sfep  is  fo  calculafe  fhe  probabilify  fhaf  fhe  sonar  would  be  refurning  fhe  reading 
given  fhe  cell  is  acfually  occupied,  P{s\H),  where  H  is  fhe  hypofhesis  fhaf  fhe  cell  is  occupied.  For 
cells  fhaf  fall  wifhin  Region  I,  fhaf  is,  in  fhe  region  of  fhe  arc  in  fhe  immediafe  vicinify  of  Q,  this 
probability  is  given  as: 

P(s|H)  =  ^  ^  X  MaXoccupM  (3.1) 

where  the  constant  MaXoccupied  represents  a  cap  on  the  certainty  that  can  be  assigned  to  any  cell, 
such  as  0.98  (the  value  used  for  fhis  research).  For  cells  fhaf  fall  wifhin  Region  II,  fhaf  is,  fhe  region 
preceding  Region  I,  we  are  reasonably  cerfain  fhey  are  empfy,  else  we  would  nof  have  received  a 
range  reading  beyond  fhem.  Thus  fhe  probabilify  fhaf  fhey  are  occupied  is  given  as  1  minus  fhe 
probabilify  of  being  occupied: 

P{s\H)  =  1  -  ^  ^  ^  ^  '  (3.2) 
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While  in  the  Murphy  model  the  absence  of  MaXoccupkd  reflects  there  is  no  need  to  cap  the  probability 
that  a  cell  is  empty,  in  this  implementation,  we  do  experiment  with  a  cap,  namely  0.5,  which 
captures  the  notion  that  cells  in  Region  II  are  assumed  to  be  empty,  thus  their  probability  of 
occupancy  should  nof  exceed  0.5.  Wifhouf  fhis  cap,  probabilities  of  occupied  wifhin  fhis  region 
are  allowed  fo  go  fo  1.0. 

Because  fhe  probabilify  fhaf  we  are  acfually  inferesfed  in  is  fhaf  of  a  parficular  cell  being 
occupied  given  a  sonar  reading,  and  nof  fhe  reverse,  Bayes'  rule  musf  be  applied: 


P{H\s)  = 


P{s\H)P{H) 

P{s\H)P{H)  +  P{shH)P{^H) 


(3.3) 


where  P{H)  and  P{^H)  are  fhe  prior  probabilifies  fhaf  fhe  cell  is  occupied  and  empfy,  respecfively, 
and  P(s|-iH)  -  1  -  P{s\H).  The  acfual  value  of  P{s\H)  will  vary  depending  upon  which  region  fhe 
cell  maps  fo,  as  discussed  above. 

As  concerns  fhe  priors,  before  fhe  environmenf  has  been  explored,  an  assumption  is  made  fhaf 
fhere  is  an  equal  probabilify  fhaf  any  cell  is  eifher  occupied  or  empfy;  fhaf  is,  P{H)  =  R(-'H)  =  0.5, 
and  fhe  entire  occupancy  grid  is  initialized  fo  0.5.  Thus  fhe  prior  is  fhe  preexisfing  value  of  fhe  cell 
under  consideration,  and  fhe  resulfing  P{H\s)  becomes  fhe  new  value  (prior)  stored  in  fhaf  cell. 


3.1.2  Mapping  Range  Data  to  the  Occupancy  Grid.  Mapping  is  performed  using  fhe  Pioneer 
P2-AT  robof,  which  has  a  parficular  sonar  configuration  and  pose  represenfafion  fhaf  do  nof 
correspond  direcfly  fo  fhe  occupancy  grid  domain. 

Firsf  among  fhese  is  fhe  manner  in  which  sonars  are  oriented  on  fhe  robof  (reference  Figure 
3.3).  The  sonars  are  numbered  clockwise  from  0  fo  15,  wifh  0  locafed  af  fhe  9  o'clock  position,  or  fhe 
wesfem-mosf  posifion  relafive  fo  fhe  robof's  heading.  Second,  fhe  heading  of  fhe  robof  is  reported 
as  being  between  0  and  +179  (inferior  numbers  in  fhe  figure),  wifh  a  posifive  value  indicating  fhe 
robof's  righf  side,  and  a  negative  indicating  fhe  leff  side.  Third,  fhe  unif  of  measure  used  for  range 
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Figure  3.3:  Pioneer  sonar  array 

readings  is  given  in  millimeters.  Lastly,  the  position  and  heading  of  the  robot,  {x,  y)  :  6,  at  the  time 
a  run  begins  are  (0, 0)  :  0°,  regardless  of  whaf  fhe  robof's  acfual  orienfafion  is  in  a  simulafed  world 
or  a  real-world  lab. 

Anofher  implicafion  of  fhis  unknown  inifial  position  is  fhaf  fhe  dafa  sfrucfure  used  fo  sfore  fhe 
grid  world  musf  fake  info  accounf  fhaf  fhe  robof  may  sfarf  af  any  exfreme  wifhin  ifs  environmenf. 
For  example,  if  ifs  inifial  position  were  fhe  boffom-leff-mosf  corner  of  a  50  x  100  world  (5  x  10 
mefers),  fhe  world  would  be  bounded  by  (0,0)  and  (50,100);  whereas  an  inifial  fop-righf-mosf 
position  would  bound  fhe  world  by  (-50,-100)  and  (0,0).  Since  fhe  inifial  orienfafion  is  unknown, 
if  a  sfafic  dafa  sfrucfure  is  used,  if  musf  be  four  times  as  large  as  fhe  acfual  world,  allowing 
for  unhindered  fravel  in  any  direcfion,  in  fhis  case,  (-50,-100)  and  (50,100).  In  order  fo  reduce 
fhis  fypically  heavy  memory  requiremenf  for  occupancy  grid  mapping,  we  chose  fo  implemenf  a 
d5mamic  sfrucfure.  Our  mufable  Carfesian  grid  sfarfs  ouf  wifh  no  dimension,  and  as  if  is  accessed, 
if  aufomafically  grows  fo  accommodafe  fhose  coordinafes  fhaf  are  oufside  ifs  currenf  bound. 

While  fhe  Pioneer  robof  refums  several  range  sweeps  per  second,  we  are  recording  only  one 
per  disfance  f raveled  wifhin  a  grid  cell,  which  is  100mm  square,  or  abouf  one  reading  every  4 
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inches.  The  reasoning  behind  this  decision  lies  in  the  consistency  of  the  sonar  noise.  As  mentioned 
in  Section  2.2,  the  reflectance  of  fhe  sonars  remains  consisfenf  fhroughouf  fhe  environmenf.  Fur- 
fhermore,  using  fhis  approach  simplifies  fhe  model  in  fhaf  only  one  sweep  need  be  mapped  fo  a 
single  cell  in  fhe  grid  world.  We  do  consider  in  Secfion  5.1,  however,  exfending  our  implemenfafion 
fo  infegrafe  mulfiple  sweeps  info  a  single  range  reading. 

Wifh  fhis  framework  in  place,  fhere  comes  fhe  fask  of  mapping  a  given  sonar  reading  fo  fhe 
grid  world  dafa  sfrucfure.  Given  is  fhe  robof's  pose,  (J,  which  as  discussed,  consisfs  of  if's  x-y 
coordinafe  in  fhe  physical  or  simulafed  world  and  if's  heading,  6,  relative  fo  ifs  inifial  heading,  as 
well  as  fhe  number  of  fhe  sonar  refurning  fhe  reading.  Each  sonar  emifs  ifs  signal  af  a  known  angle. 
Thus,  using  fhe  robof  heading  and  sonar  heading,  fhe  posifion  of  fhe  objecf  can  be  defermined 
using  fhe  simple  coordinafe  fransform: 


Xg 

=  Xy  +  dg  Sin(0,.  +  dyg) 

(3.4) 

Vs 

=  yy  +  dg  COS{dr  +  dyg) 

(3.5) 

Xg 

=  Xg  +  dg  sin(0r  +  0s) 

(3.6) 

Vo 

=  1/s  +  do  COS(dy  +  dg) 

(3.7) 

where  {Xy,  y^),  {Xs,  ys),  and  {Xg,  yo)  idenfify  fhe  coordinafes  of  fhe  robof,  sonar  device,  and  objecf, 
respectively;  and  dy,  dys,  and  dg  idenfify  fhe  heading  of  fhe  robof,  fhe  angle  from  fhe  robof  confer 
fo  fhe  sonar  device,  and  fhe  heading  of  fhe  sonar  signal  relative  fo  fhe  robof,  respectively  If  is 
fhis  grid  world  coordinafe  {Xg,  yg)  fhaf  corresponds  fo  fhe  objecf  and,  along  wifh  fhe  grid  world 
coordinafe  of  fhe  robof's  confer  poinf,  serves  as  fhe  frame  of  reference  for  updafing  fhe  occupancy 
grid  wifh  fhe  resulf  of  Equation  (3.3). 

3.2.3  Building  the  Occupancy  Grid.  There  are  fhree  increasingly  complex  approaches,  or 
models,  for  updafing  fhe  occupancy  grid  fhaf  are  implemenfed  in  fhis  research: 
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Figure  3.4:  Mapping  the  sonar  probability  model  using  polygon  filling 


Point  of  Return  Update  the  cell  corresponding  to  the  range  reading,  Q 
Acoustic  Axis  Update  all  the  cells  that  fall  along  the  sonar's  acoustic  axis 
Field  of  View  Update  all  the  cells  within  the  cone  of  the  sonar  beam 

The  first  approach  has  the  benefit  of  being  extremely  fast,  quite  simple,  and  rather  effective, 
at  least  for  generating  a  good  estimate  of  the  environment  (discussed  in  Chapter  4).  It  is  a  version 
of  the  HIMM  approach  discussed  in  Section  2. 2. 1.1,  with  the  difference  that  it  uses  probability 
values  rather  than  histograms. 

The  second  approach  employs  the  Bresenham  Line  Algorithm  [46]  to  traverse  the  cells  along 
the  acoustic  axis,  that  is,  between  (Xg,  yo)  and  {Xy,  yr)  (Figure  3.4).  As  each  cell  is  traversed,  the  cell 
is  filled  with  its  probability  of  occupancy,  taking  into  account  its  distance  r  from  the  sensor  (the 
angle  a  off  the  acoustic  axis  is  always  0  for  this  method). 

The  third  approach,  also  illustrated  in  Figure  3.4,  represents  a  full  implementation  of  the 
probabilistic  occupancy  grid  in  that  it  prescribes  that  all  cells  within  the  sonar  cone  have  probabil¬ 
ities  associated  with  them  for  a  particular  reading.  For  this  problem,  a  polygon  filling  routine  is 
implemented,  where  two  polygons  are  defined  around  Regions  I  and  II,  as  defined  by  the  points 
shown  in  Figure  3.4. 
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Figure  3.5:  Scan  line  polygon  fill 


There  are  three  considerations  that  went  into  choosing  an  appropriate  polygon  filling  algo¬ 
rithm,  namely;  that  it  be  efficient,  complete,  and  non-redundant.  Complete  means  that  all  cells 
within  the  polygon  and  between  adjacent  polygons  must  be  filled  and  fhere  may  be  no  gaps.  Non- 
redundanf  means  no  cell  be  filled  more  fhan  once  (even  fhose  fhaf  share  a  common  border  befween 
fwo  polygons),  for  in  our  recursive  probabilisfic  occupancy  grid  confexf,  a  mulfiply-updafed  cell 
will  resulf  in  an  arfificially  high  or  low  occupancy  value.  An  algorifhm  fhaf  fulfills  all  fhree  re- 
quiremenfs  is  fhe  Abrash  implemenfafion  of  fhe  Scan  Line  Algorifhm  [1].  As  shown  in  Figure 
3.5,  fhe  concepf  enfails  verfically  moving  a  horizonfal  scan  line  over  fhe  polygon  in  one  direcfion, 
filling  fhe  cells  fhaf  fall  wifhin  fhe  boundaries  of  fhe  polygon  lines  if  infersecfs,  excluding  fhose 
cells  fhaf  fall  on  fhe  off  borders. 

3.2  Localization 

Localizafion  describes  fhe  procedure  by  which  corrections  are  made  fo  fhe  robof's  pose  such 
fhaf  ifs  perceived  location  mafches  as  closely  as  possible  wifh  ifs  acfual  position.  The  furfher  a 
robof  fravels,  fhe  greafer  fhe  error  in  ifs  expecfed  pose.  This  error  is  mosf  apparenf  in  cyclical 
environmenfs,  such  as  when  fhe  robof  maps  a  room  beginning  and  ending  af  fhe  same  poinf,  or 
fravels  fhrough  a  corridor  fhaf  circles  back  fo  fhe  sfarfing  position.  The  latter  is  mosf  difficulf 
because  in  fhis  sifuafion,  fhere  is  no  prior  knowledge  in  fhe  map — ^no  overlap  or  backfracking,  so 
fo  speak,  fhaf  assisfs  in  defermining  an  accurafe  pose. 
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The  localization  approach  used  for  this  research  is  largely  based  upon  the  models  presented 


in  [10,43,45]. 

3.2.1  Statistical  Basis.  The  problem  of  concurrent  mapping  and  localization  is  here 
represented  as  a  maximum  likelihood  estimation  problem,  where  a  number  of  expected  poses  for 
each  local  map  given  the  data  in  the  global  map  are  found,  and  the  maximum  of  those  is  selected. 

3.2.1.1  The  Data  Model.  Following  the  nomenclature  of  [10],  the  stream  of  sensor 
data  received  from  the  robot  is  represented  by 

d  =  (o\  0^,  M^, ...,  0^,  u^],  (3.8) 

where  o*  and  u*  are  a  sweep  of  sonar  observations  and  robot  control  commands  at  time  t,  respec¬ 
tively,  the  total  number  of  time  steps  being  represented  by  T.  For  this  implementation,  a  control 
command  at  time  t  is  equivalent  to  the  robot's  pose  at  that  time  (reported  by  the  robot's  internal 
position-speed  encoders),  because  the  Pioneer  does  not  take  discrete  commands  such  as,  "turn  left 
10  deg  and  move  forward  100cm,"  but  is  rather  controlled  by  a  keyboard  or  joystick  interface. 

A  map,  denoted  m,  is  an  assignment  of  properties  to  j-y-locations  in  the  world.  While 
for  [43],  m  contains  knowledge  of  the  exact  location  of  all  landmarks,  for  [10]  these  landmarks 
are  replaced  by  local  maps,  which  is  the  implementation  used  here.  Furthermore,  for  the  metric 
occupancy  grid  approach  used  here,  these  properties  are  probabilities  that  collectively  represent 
obstacles. 


3.2.1.2  The  Motion  Model.  The  motion  model,  denoted  P(^'|m, <5),  describes  the 
probability  of  the  current  pose  <£'  given  the  prior  pose  cj  and  motion  control  command  u.  In  the 
context  of  local  maps,  the  prior  pose  is  that  of  the  prior  local  map,  and  the  motion  control  command 
is  a  representation  of  the  travel  conducted  over  the  course  of  that  map.  This  model  captures  the 
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(a)  (b) 

Figure  3.6:  The  motion  model,  showing  the  further  the  distance  of  robof  fravel,  fhe  greafer  fhe 

pose  disfribufion's  depfh;  and  fhe  greafer  fhe  degrees  of  fum  made  by  fhe  robof,  fhe  greafer  fhe 
disfribufion's  widfh. 

normally  disfribufed  uncerfainfy  in  fhe  robof's  known  pose  fhaf  increases  fhe  furfher  fhe  robof 
fravels  and  fhe  greafer  fhe  degree  of  fums  if  makes,  as  illusfrafed  by  Figure  3.6.  The  shaded  area 
shows  fhe  pose  disfribufion  (projecfed  info  fwo  dimensions)  affer  fhe  pafh  indicafed  by  fhe  solid 
line  has  been  fraveled,  fhe  degrees  of  gray  reflecting  fhe  pose  likelihood. 

In  fhis  implemenfafion,  four  facfors  go  in  fo  fhe  consfrucfion  of  fhis  model  for  any  given 
local  map  pose: 

•  heighf,  which  retiecfs  fhe  robof's  disfance  of  fravel  in  fhe  local  map 

•  widfh,  which  retiecfs  fhe  robof's  degree  of  fum  over  fhe  course  of  fhe  local  map 

•  cr,  a  Gaussian  scale  paramefer  for  which  a  higher  value  sfrefches  fhe  disfribufion  (values  in 
fhe  periphery  are  higher;  fhe  descenf  is  slower;  cr  =  1  for  a  sfandard  normal  disfribufion) 

•  T,  which  specifies  fhe  infensify  of  fhe  modeFs  friangular  shape 

Two  Gaussian  disfribufions  corresponding  fo  fhe  widfh  and  heighf  are  used  fo  form  fhe  disfri¬ 
bufion;  a  is  applied  fo  bofh.  Wifhouf  t,  fhe  resulting  disfribufion  would  be  elliptical;  wifh  t,  fhe 
sides  of  fhe  disfribufion  are  effectively  pulled  down  fo  form  a  friangular  shape.  Picfuring  fhe 
disfribufion  as  a  mafrix,  fhis  is  accomplished  by  shifting  enfire  columns  down  by  a  factor  of  fhe 
column's  disfance  from  fhe  disfribufion's  confer  and  of  z.  Thus,  fhose  columns  on  fhe  outer  edges 
are  shifted  by  fhe  greafesf  amounf;  fhose  toward  fhe  confer  are  shifted  only  slighfly.  A  greafer  z 
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produces  a  greater  shift.  While  the  width  and  height  are  factors  of  fhe  robof's  fravel,  a  and  t  are 
sfafic,  yef  configurable,  sysfem  paramefers. 

3.2.1. 3  The  Perception  Model.  This  model,  denofed  as  P{o\m,  Q,  models  fhe  likelihood 
of  observing  o  in  sifuafions  where  bofh  fhe  world  map  m  and  robof  pose  ^  are  known.  Here,  fhis 
uncerfainfy  in  fhe  sonar  reading  is  being  capfured  by  fhe  Murphy  model  previously  described, 
which  is  largely  a  facfor  of  range  reading  disfance,  and  less  so  of  a  cell's  disfance  off  fhe  acousfic 
axis,  when  considering  fhe  full  sonar  cone. 

These  fhree  quanfifies:  fhe  sef  d  of  dafa,  fhe  mofion  model  P{^'\u,^),  and  fhe  perception 
model  P{o\m,  ^),  form  fhe  sfafisfical  basis  for  our  localization  approach. 

3.2.1.4  The  Map  Likelihood  Function.  The  problem  of  mapping  can  be  sfafed  as  fhe 
problem  of  finding  fhe  mosf  likely  map  given  fhe  dafa,  fhaf  is,  m’  =  argmaxP(m|d).  This  probabilify 

m 

can  be  derived  as  follows  info  fhe  separafe  mofion  and  percepfion  models  used  for  our  localization 
approach: 


m* 

P{m\d) 

p{m\e,...,e,d) 

p{d\m,e,.:,e) 

p{e,...e\d) 

P{m\d) 


argmax  P{m\d) 

m 

J  p{m\e,...,e,d)  p{e,...,e\d)  de...de 

p{d\m,e,.:,e)  p{m\e,:.,e) 

p(d\e,..;^^) 

T 

llP{o^\m,e) 

t=l 

flp{e^^\u\e) 

t=i 


(3.9) 

(3.10) 

(3.11) 

(3.12) 

(3.13) 


T 


T-1 


[]P(oV,5')  P(m)  de...de{3.U) 

t=l  t=l 


T  T—1 

argmax  f ...  f  TT  P(oV,  <5')  n  ^’(■5*'^^ 

m  J  J  t=\  t=l 


w,e)  de...de 


(3.15) 


where. 
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•  P{m\d)  in  (3.9)  can  be  written  as  (3.10)  by  explicitly  specifying  and  integrating  over  the  pose 
information,  recognizing  that  the  map's  likelihood  is  dependent  upon  those  poses,  and  by 
applying  Bayes'  rule 

•  the  first  term  on  the  right  hand  side  of  (3.10)  can  be  rewritten  as  (3.11)  via  anofher  application 
of  Bayes'  rule 

•  fhe  firsf  ferm  on  fhe  righf  hand  side  of  (3.11)  can  be  fransformed  fo  (3.12),  based  on  fhe 
observation  fhaf  o*  depends  only  on  fhe  map  m  and  fhe  locafion  af  fime  t,  and  nof  on  prior 
observations 

•  fhe  second  ferm  on  fhe  righf  hand  side  of  (3.11)  may  be  considered  as  P{m)  since  in  fhe 

absence  of  any  dafa,  m  does  nof  depend  on  any  of  fhe  locafions  where  P{m)  is  fhe  Bayesian 

prior  over  all  maps,  which  is  assumed  fo  be  uniformly  disfribufed 

•  fhe  second  ferm  in  (3.10)  can  be  rewritten  as  (3.13),  based  on  fhe  observation  fhaf  fhe  robof's 
locafion  depends  only  on  fhe  robof's  locafion  one  fime  sfep  earlier  and  fhe  action  u* 
execufed  af  fhaf  poinf 

•  subsfifufing  (3.11),  (3.12),  and  (3.13)  info  fhe  righf  hand  side  of  (3.10)  leads  fo  fhe  likelihood 
function  (3.14),  and 

•  (3.15)  resulfs  from  subsfifufing  (3.14)  back  info  (3.9)  while  dropping  P(m)  and  1/P(d|^^, ...,  <5^), 
since  we  are  only  inferesfed  in  maximizing  P{m\d)  and  nof  in  compufing  ifs  value 

The  final  equation  (3.15)  is  exclusively  a  function  of  fhe  dafa  d,  fhe  percepfual  model  P{o\m,  Q,  and 
fhe  motion  model  P{^'\u,  Q,  all  previously  described.  Maximizing  fhis  expression  is  equivalenf  fo 
finding  fhe  mosf  likely  map.  [43] 

3.2.2  Layered  Map  Representation.  To  assisf  wifh  real-time  localization,  occupancy  grid 
values  are  mapped  fo  a  series  of  local  maps  which  overlay  afop  a  single  global  map,  according  fo 
fhe  fheory  of  [10],  which  argues  fhaf  fhe  independence  assumption  inherenf  in  occupancy  grids 


34 


Figure  3.7:  Bi-modal  pose  distribution 


can  be  avoided  by  adopting  a  richer,  layered  representation  using  local  maps.  The  problem  they 
describe  is  illustrated  in  Figure  3.7:  Suppose  a  series  of  robot  sensor  readings  suggest  a  map  like 
that  shown  on  the  left  side,  but  it  has  two  distinct  h5rpothesis  as  to  where  it  might  be.  Integrating 
this  local  map  into  a  single  global  map  in  relation  to  those  two  hypothesis  may  yield  a  map  like 
that  shown  on  the  right.  Such  maps  are  not  usable  for  localizafion  wifh  proximify  sensors,  and  fhe 
sifuafion  worsens  when  the  robot  is  more  globally  uncertain,  resulting  in  maps  that  often  contain 
no  visible  structure. 

The  local  maps  address  this  problem.  Each  map  represents  the  range  data  received  over 
a  portion  of  fhe  global  map.  Each  map  mainfains  a  pose  fhat  is  faken  as  fhe  robof  pose  in  the 
global  map  at  the  time  of  creafion,  refains  all  fhe  sonar  range  readings  wifh  which  if  is  builf,  and 
f racks  fhe  disfance  and  degrees  of  fum  fhe  robof  fraveled  wifhin  ifs  bounds.  While  fhe  pafh  of 
fhe  robof  wifhin  each  local  map  is  unique  fo  fhaf  map  (if  is  nof  repeafed  in  any  ofher),  fhere  is 
overlap  between  fhem  due  fo  fhe  reach  of  fhe  sonars.  Af  consfrucfion,  a  local  map  is  localized 
based  upon  fhe  global  map  as  if  exisfs  af  fhaf  poinf .  If  is  fhen  builf  using  sonar  dafa  for  a  prescribed 
disfance  and  relocalized  using  fhe  newly  acquired  dafa.  This  process  repeafs  for  fhe  duration  of 
fhe  mapping  session. 

Rafher  fhan  mainfain  a  global  map  fhaf  convolves  fhe  local  maps  info  a  single  monolifhic 
map  as  fhey  are  creafed,  fhe  local  maps  are  d5mamically  convolved  whenever  a  cell  in  fhe  global 
map  is  accessed.  Eor  example,  if  fwo  maps  overlap  af  cell  (x,  y),  bofh  maps  are  consulfed  for  fheir 
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Figure  3.8:  Overlapping  local  maps  regions,  showing  construction  of  regions  and  mapping  global 
coordinates  to  those  local  maps  that  cover  the  coordinates. 
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Algorithm  1  Local  map  construction 
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procedure  MApToREGioNs(m[;]  :  m,  [R],  r) 
bi  <—  m[,]  .bound 
h  r[o] 

for  all  m[y]  y=,_i  0  do 
hij  <—  hi  n  m[y]  .bound 
if  hij  +  0  then 

for  all  cells  k  e  bp  do 

n  <-  [R]k 

ifr^^h  then 

bijk  <—  Pi  .bound  n  bij 
bi  ^  bi  —  bijj^ 

rj:  .bound  r*:  .bound  -  bijk 
if  rj:  .bound  =  0  then 
DELETEREGION(ri:) 

end  if 

b/i:  NEwREGION(fc,yj.) 

Tijk. maps 
h  «  {rk,rijk} 

end  if 
end  for 
end  if 
end  for 
if  bi  +  0  then 

Yi  <—  NEwREGION(fc,) 
fj.maps  m[i] 
r  «  Ti 
end  if 

end  procedure 


29:  function  NEwREGioN(fc„  :  [R],  r) 
30:  r„  <-  (lD(),fc„} 

31:  r  <^r„ 

32:  for  all  cells  I  e  r„  .bound  do 

33:  [R]l  <r-  Tn 

34:  end  for 

35:  return  r„ 

36:  end  function 


37:  procedure  DELETEREGioN(rc; :  [R],  r) 

38:  for  all  cells  I  e  .bound  do 

39:  [R]i  ^  To 

40:  end  for 

41:  [R]  »  ra 

42:  delete  ra 

43:  end  procedure 
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independent  prior  probabilities  of  occupancy  at  that  cell,  the  values  convolved  (averaged),  and 
the  result  returned.  At  no  time  during  map  construction  or  localization  are  these  values  statically 
represented. 

To  accomplish  this,  the  global  map  is  represented  not  by  priors,  but  by  region  identifiers.  A 
region  is  defined  as  an  area  of  fhe  global  map  fhaf  is  uniquely  covered  by  zero  or  more  local  maps. 
Before  any  mapping  has  begun,  fhe  enfire  map  is  covered  by  region  0  (ro).  When  a  map  has  been 
consfrucfed,  if  is  regionalized  in  fhe  sense  fhaf  if  is  overlayed  on  fhe  global  region  map,  and  any 
infersecfions  wifh  pre-exisfing  regions  are  merged  info  new  regions.  A  fable  lookup  is  mainfained 
for  each  region  fhaf  poinfs  back  fo  all  overlapping  local  maps.  In  fhis  way,  fhose  maps  can  be 
insfanfly  queried  for  fheir  priors,  and  fhose  priors  convolved. 

This  process  is  illusfrafed  by  Figure  3.8  and  defailed  by  Algorifhm  1}  In  summary,  fhe  figure 
shows  fhe  insfallmenf  of  fhree  local  maps,  divided  info  fhree  major  columns,  each  insfallmenf 
corresponding  fo  one  call  fo  MapToRegions().  The  fhird  map  in  minor  columns  (3a)  and  (3b) 
represenfs  fwo  iferafions  over  fhe  for  loop  beginning  on  line  4  of  fhe  algorifhm  since  fhere  exisf 
af  fhis  poinf  fwo  prior  local  maps  wifh  which  fhe  currenf  map  needs  fo  be  merged.  Horizonfally 
fhe  figure  is  broken  info  fhree  major  rows.  The  firsf  simply  shows  fhe  local  maps  and  robof  pafh 
over  fhose  maps  as  fhey  exisf  af  fhe  call  of  MapToRegions(),  reflecfed  in  fhe  paramefers  m[,],  fhe 
currenf  local  map,  m,  fhe  collection  of  local  maps;  [R],  fhe  global  region  map  (a  mafrix);  and  r,  fhe 
collecfion  of  regions.  For  example,  fhe  firsf  row  of  column  (2)  shows  m[2]  overlapping  fW[i].  The 
second  row  models  lines  2  fhrough  21  of  fhe  algorifhm,  where  fhe  boundary  of  fhe  currenf  map  is 


^The  nomenclature  used  in  the  algorithm  is  as  follows.  Parameter  lists  are  divided  into  two  parts:  the  first  being  those 
parameters  that  are  passed  in  as  part  of  the  procedure  or  function  call;  and  the  second  being  those  parameters  that  are 
available  outside  the  scope  of  the  procedure,  and  thus  not  part  of  the  formal  parameter  list.  Atomic  values  (those  that  are 
not  collections)  are  represented  by  a  single  letter  with  or  without  unbracketed  subscripts,  such  as  fo;  or  r/y;;  and  assigned 
using  .  Vectors  or  collections  are  represented  as  a  whole  by  a  single  letter  or  an  over-arrow  word,  in  part  by  a  bracketed 
index  »![,],  and  inserted  into  or  extracted  from  using  «  and  »,  respectively.  R-values  containing  multiple  items  are  enclosed 
in  curly  braces;  for  example,  line  18  is  inserting  two  regions  into  the  history  vector  h.  Maps,  which  are  represented  as 
matrices,  are  identified  by  an  uppercase  identifier  surrounded  by  square  brackets,  as  in  [R].  Finally,  some  atomic  values 
are  actually  structures  of  related  values.  Regions  are  an  example  of  this;  they  contain  an  implied  identification  number,  a 
boundary,  and  a  collection  of  local  maps  to  which  they  correspond.  These  values  are  accessed  using  dot  notation:  r;t -bound 
or  r[,].maps. 
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first  intersected  with  a  prior  local  map,  and  the  result  is  iteratively  intersected  with  the  regions  it 
overlays.  For  example,  in  the  second  row  of  column  (3a),  the  boundary  of  map  mpj  is  first  shown 
intersecting  the  portion  of  region  rp]  from  column  (2)  that  falls  within  the  boundary  of  fW[2],  the 
prior  local  map  currently  under  consideration.  It  also  shows  below  that  its  intersection  with  r[3]  as 
the  next  region  falling  within  the  boundary  of  mpj .  The  third  row  of  the  figure  shows  the  global 
region  map  [R]  as  it  exists  after  all  the  regions  over  the  prior  local  map  have  been  processed;  that 
is,  upon  completion  of  the  for  loop  ending  on  line  20.  The  regions  are  identified  by  shade  and 
given  with  the  local  maps  to  which  they  point. 

Formally,  a  map  m  is  a  conjunction  of  N  small  maps,  denoted  m[,],  and  annotated  by  a 
coordinate  transform,  denoted  <5[i]: 


m 

(3.16) 

M[/] 

(3.17) 

ni 

(3.18) 

The  origin  of  each  map  is  locally  (0,0);  the  transform  is  a  mapping  of  the  i-th  local  map's  pose  (£[,]  to 
a  global  pose  for  that  map,  mapping  its  origin  to  a  global  coordinate  and  heading.  It  is  because  the 
local  maps  are  not  convolved  with  their  pose  distributions  into  the  global  map  that  the  problems 
arising  from  using  a  single  map  are  eliminated. 

With  the  introduction  of  the  layered  maps,  the  perceptual  model  P{o\m,  <5)  must  be  modified. 
In  order  to  extend  this  perceptual  model  to  this  map  representation,  [10]  makes  the  conditional 
independence  assumption: 


P(M[,]M[;]|o,5)  =  P(M[,]|o,5)  P(M[;]|o,^) 


(3.19) 
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for  all  i  i=  j.  This  assumption  allows  the  extension  of  the  perceptual  model  in  the  following  manner: 

N 

Pm,m)  =  P(o|5,(g)M[,]) 

i=l 

P{o\^,®l^Mu]) 

(,)  nl^p{Miii\o,QPm 
P(MhI5) 

(c)  1  Ap(Mh|o,^)P(o|^) 

Pm(^-P  P(MhI5) 

(d)  1  pm) 

P{o\^)(N-P  U  P{o\Q  P{Mii]\Q 

N 

=  r][]P(o|M[,],^)  (3.20) 

1=1 

where, 

•  the  symbol  represents  the  convolution  over  the  local  maps  M[i  jv] 

•  the  derivation  to  (a)  follows  from  Bayes'  rule 

•  fhe  derivafion  fo  (b)  represenfs  fhe  convolution  of  fhe  maps  as  fhe  producf  of  fheir  individual 
probabilities 

•  fhe  normalizafion  consfanf  l/P(o|^)^“^  in  (c)  is  independenf  of  fhe  map  and  may  be  removed 
from  consideration 

•  fhe  derivafion  fo  (d)  follows  from  Bayes'  rule  applied  fo  fhe  firsf  ferm  P(M[,]|o,  of  (c),  and 

•  (3.20)  resulfs  from  cross-cancelafions  in  (d) 

Here  fhe  expression  P{o\^,  M[,])  is  compufed  using  fhe  perception  model  described  in  Secfion  3.2.1 .3. 
The  approach  compufes  fhe  likelihood  of  observafions  separafely  in  all  maps,  and  fhen  combines 
fhe  resulting  densify  mulfiplicafively  [10]. 
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3.2.3  Expectation  Maximization.  As  was  discussed  in  Section  2.2,  there  exists  a  high 
dimensionality  to  the  mapping  problem.  As  a  robot  travels,  the  range  of  possible  poses  and 
possible  maps  grows  to  a  point  that  is  computationally  prohibitive  to  exhaustively  process.  EM  is 
a  statistical  approach  to  reducing  the  scope  of  fhe  problem  in  such  a  way  fhaf  reasonable  resulfs 
can  be  achieved  wifhouf  fhe  explicif  consideration  of  all  possible  map  configurations. 

3.2. 3.1  Expectation  Step.  In  fhe  confexf  of  localizafion,  fhe  expecfafion  sfep  describes 
fhe  process  by  which  a  sef  of  expecfed  poses  are  calculafed  for  a  parficular  robof-reporfed  pose, 
reflecting  fhe  facf  fhaf  fhere  is  uncerfainfy  in  a  pose  and  fhaf  fhe  reporfed  pose  may  nof  besf  fif  fhe 
dafa. 


Theory.  In  fhe  E-sfep,  fhe  currenf  besf  map  m  and  fhe  dafa  are  used  fo  compufe 
probabilistic  esfimafes  for  fhe  robof's  position  <5^  af  f  =  1,..., T,  fhaf  is,  P{^^\d,m),  which  per  [43], 
can  be  expressed  as  fhe  normalized  producf  of  two  ferms  in  fhe  following  marmer: 


P{e\d,m) 


=  P(<5'|o^...,o',M^...,o^m) 

=  rji  P(o\ ...,  o'|<5^  u‘, ...,  0^,  m)  P(5 ••  •,  m) 

=  rji  P(o\ ...,  o'|<5^  m)  ...,  m) 

-  ??2  P{^^\o^,  •••/  m)  P(o^, ...,  o‘lm)  P((5‘|m*,  ...,  o^,  m) 

=  ri3  P(^'|o^ ...,  o^  m)  ...,  m) 


:=a^ 


(3.21) 


The  normalizers  Mi,  M2,  and  M3  ensure  fhe  leff-hand  side  of  fhe  equafion  sums  up  fo  one  over 
all  The  derivation  of  3.21  follows  from  (a)  Bayes  rule,  (b)  a  commonly -used  Markov  assumption 
fhaf  specifies  fhe  conditional  independence  of  fufure  dafa  from  pasf  dafa  given  knowledge  of  fhe 
currenf  locafion  and  fhe  map,  and  (c)  Bayes  rule  under  fhe  assumpfion  fhaf  in  fhe  absence  of  dafa, 
robof  positions  are  equally  likely. 
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Both  a  and  jS,  analogous  to  the  forward  and  backward  functions  of  fhe  Hidden  Markov 
Model  (HMM)  alpha-befa  algorifhm  [35,36],  are  compufed  separafely.  While  fhe  compufafion 
of  fhe  a-values  is  a  version  of  Markov  localizafion  using  mulfiple  local  maps,  fhe  jS-values  add 
addifional  knowledge  abouf  fhe  robof's  position — nof  f5rpically  capfured  by  Markov  localizafion, 
buf  essenfial  for  revising  pasf  belief  based  on  sensor  dafa  fhaf  was  received  lafer  in  fime  [43]. 

The  compufafion  of  a*  is  as  follows,  wifh  fhe  exception  of  fhe  bound  which  akin  fo  fhe 
initial  sfafe  disfribufion,  n,  in  [36],  is  1  af  fhe  inifial  pose  (0,0)  :  0°. 


a 


t 


m) 

rj  P(o‘\^^,  o^ ...,  m)  P{^‘\o^, ...,  m) 
r]  m)  P{^^\o^, ...,  u‘~^,  m) 

where, 

J  p(^‘iu‘-\e-^)  p(e-v.:.,o‘-\m)  de-^ 


(3.22) 


(3.23) 


Subsfifufing  3.23  back  info  3.22  yields  a  recursive  rule  for  fhe  compufafion  of  all  a‘,  which  uses 
fhe  dafa  d,  fhe  model  m,  as  well  as  fhe  mofion  model  P(.^'|w,  and  fhe  percepfual  model  P{^\o,  m), 
defailed  in  Secfions  3. 2. 1.2  and  3. 2. 1.3,  respectively  This  sfafes  fhaf  a  af  fime  t  is  equal  fo  fhe 
normalized  producf  of  fhe  observation  model  af  fime  t  mulfiplied  by  fhe  summed  producf  of  fhe 
mofion  model  af  fime  t  and  a  af  fime  f  -  1,  wifh  summation  over  fhe  mofion  model  af  fime  f  -  1. 

The  compufafion  of  which  expresses  fhe  probabilify  fhaf  fhe  robof's  final  pose  is  <5,  is 
complefely  analogous  fo  a,  buf  backward  in  fime.  Since  does  nof  depend  on  fhe  dafa,  fhe  inifial 
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is  uniformly  distributed.  For  all  others. 


=  J  (3.24) 

where, 

=  r]P(o'+i|5'+i,m)|S'+i  (3.25) 


The  result  of  the  E-step,  a‘p‘,  is  an  estimate  of  the  robot's  locations  at  the  various  points  in 
time  t. 


0  1  2  3  f=4  5  7=6 


ji 

Figure  3.10:  Modified  calculation  of  a  and  p  in  HMM-based  EM 

The  manner  in  which  a  and  p  are  calculated  is  illustrated  by  Figure  3.9.  T  represents  the  total 
number  of  maps,  that  is,  the  map  collected  at  the  last  iteration  T.  The  entire  map  is  available  before 
the  localization  procedure  begins.  Iteration  occurs  from  t  =  0  to  t  =  T.  At  each  t,  a  is  computed 
over  Mo.  f  and  jS  is  computed  over  Mf.  r-  For  example,  in  the  figure,  t  has  progressed  from  t  =  0  to 
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t  =  A,  and  at  each  step,  a  and  jS  have  been  recalculated.  This  process  repeats  at  each  step  t,  which 
is  why  this  algorithm  is  executed  off-line. 

Implementation.  A  key  difference  between  our  implementation  and  the  work 
of  [10]  is  that  between  concurrent  and  real-time  mapping  and  localization.  The  mapping  procedure 
described  in  Section  4  of  that  article  states  all  local  maps  are  built  before  the  EM  procedure  begins. 
This  is  how  they  are  able  to  fully  implement  the  f  step.  In  contrast,  a  primary  focus  of  our  research 
is  real-time  localization,  which  limits  our  ability  to  fully  implement  these.  Our  procedure  is  to 
instead  perform  the  forward  a  calculation  as  the  map  is  being  built,  localizing  the  initial  pose  of  a 
map  at  the  point  of  that  map's  creation,  and  then  to  perform  the  backward  jS  step  over  the  previous 
iteration,  as  illustrated  in  Figure  3.10. 

For  both  a  and  f,  the  Bayesian  occupancy  grid  model  described  in  section  3.1  serves  to 
capture  in  large  measure  the  utilized  probabilities.  That  is,  because  the  grid  is  built  incrementally 
one  observation  at  a  time,  and  each  observation  builds  on  that  of  the  prior  observation,  the 
probability  of  a  given  pose  can  be  said  to  be  conditionally  dependent  upon  the  map  as  it  is 
currently  represented. 

The  manner  in  which  the  expected  pose  is  found  is  very  similar  to  that  described  in  [45].  In 
that  work,  the  terminal  end  of  the  sonar  range  vectors  (that  is,  the  end  at  the  point  of  the  range 
reading)  are  compared  against  all  occupied  cells  within  the  vicinity  of  the  expected  robot  pose,  and 
those  for  which  the  vector  has  an  unobstructed  path  contribute  to  a  collection  of  feasible  poses.  In 
this  work,  these  vectors  are  aligned  at  their  originating  end,  that  is,  at  the  point  of  the  sonar,  over  a 
normally  distributed  set  of  possible  sonar  poses,  as  determined  using  a  motion  model,  previously 
discussed. 

This  probabilistic  motion  model  is  computed  as  a  Cartesian  grid  with  the  same  resolution 
as  the  occupancy  grid  and  with  its  origin  at  the  reported  robot  position,  as  corrected  by  previous 
localizations.  The  cumulative  distance  and  degrees  of  turn  reported  since  the  last  localization 
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Figure  3.11: 


Sample  pose  distribution  over  the  motion  model 


determine  the  dimension  of  this  pose  distribution;  a  greater  distance  increases  its  height,  a  greater 
degree  of  fum  increases  ifs  widfh  (Figure  3.6). 

Figure  3.11  illusfrafes  fhis  procedure.  The  figure  shows  a  section  of  a  map  af  fhe  poinf  of  a 
localizafion,  wifh  fhe  motion  model  pose  disfribufion  overlaid  on  fop,  highlighfed  by  fhe  boxed 
region.  The  robof,  sonar,  and  objecf  posifions  are  annofafed  wifh  an  R,  S,  and  O,  respecfively.  The 
pose  disfribufion  is  cenfered  on  fhe  robof  pose.  The  vector  fhaf  extends  from  fhis  center  to  fhe 
objecf  is  mapped  from  every  cell  in  the  pose  distribution  to  a  prospective  range  reading  cell.  This 
is  illustrated  by  the  single  row  of  mappings  shown.  All  buf  one  of  fhese  represenfs  a  feasible  pose; 
fhe  one  interrupted  by  an  X  indicafes  a  reading  obsfrucfed  by  fhe  cell  wifh  a  prior  probabilify 
of  occupancy  of  0.92.  If  fhe  pafh  is  unobsfrucfed  (as  per  fhe  currenf  global  map),  fhe  normally- 
disfribufed  probabilify  of  fhe  cell  af  fhe  terminal  end  of  fhat  vector  being  occupied  is  convolved 
wifh  fhe  underlying  map's  probabilify  of  occupancy  af  fhat  same  location  using  the  Murphy  model 
described  in  Section  3.1.1.  This  pose  likelihood  value  is  retained  in  a  second  probabilistic  Cartesian 
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grid  that  is  likewise  aligned  with  the  motion  model  over  the  global  map  at  the  robot's  pose.  These 
likelihood  values  are  used  in  the  maximization  step. 

3. 2. 3.2  Maximization  Step. 

Theory.  The  M-step  calculates  the  most  likely  poses  of  the  local  maps  given 
the  data  and  current  best  map.  Letting  f[,]  be  the  index  of  fhe  firsf  dafa  poinf  in  fhe  i-fh  local  map, 
which  corresponds  fo  fhaf  map's  pose,  fhe  M-Sfep  calculafes: 

^[i]  =  argmax  |d,  m) 

=  argmax  (3.26) 

Because  EM  is  a  hill  climbing  mefhod,  which  only  converges  fo  a  local  maxima,  if  fhe  odomefric 
error  is  large,  fhe  inifial  map  will  be  erroneous,  and  subsequenf  iferafions  of  EM  mighf  nof  be  able 
fo  recover  (as  in  Eigure  2.1  on  page  7).  This  is  exfended  in  [10]  by  generafing  a  disfribufion  over 
fhe  models  fhaf,  using  deferminisfic  annealing,  slowly  converges  fo  fhe  mosf  likely  map  pose;  buf 
because  of  our  real-fime  consfrainfs,  fhis  refinemenf  is  excluded. 

Implementation.  Affer  all  pose  likelihoods  have  been  evaluafed  for  a  sonar,  a 
fhird  Carfesian  grid  fhaf  collecfs  histogram  rafher  fhan  probabilisfic  values  is  created.  This  grid  is 
aligned  over  fhe  global  map  af  fhe  reporfed  robof  pose  and  is  updafed  in  fhe  following  manner. 
Each  cell  fhaf  corresponds  fo  a  cell  in  fhe  pose  likelihood  grid  wifh  a  maximum  likelihood  value 
(fhe  sonar's  vote  for  fhe  mosf  likely  robof  position)  is  incremented  by  one.  This  same  hisfogram 
is  independenfly  updafed  for  each  sonar.  Once  all  sonars  in  a  single  sonar  sweep  are  processed, 
fhose  cells  wifh  fhe  maximum  value  indicate  all  possible  localized  poses. 

Erom  fhese  selecfed  poses,  fhose  nof  associated  wifh  fhe  maximum  selected  pose  disfribufion 
are  filfered  ouf.  If  more  fhan  one  remains,  fhaf  which  is  closesf  fo  fhe  original  pose  is  selecfed. 
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Algorithm  2  Local  map  localization 


1:  procedure  LocALizE(m[,]) 

2:  [A]^,^P{^'\U,0 

3:  [CU  ^  0 

4:  for  all  sonars  i  do 

5:  r[i]  <—  range  reading 

6:  Pr{Occ)  <—  [Map]i 

7:  [B]  ^  0 

8:  for  all  cells  j  in  [A]  do 

9:  if  not  obstructed(r[,y])  then 

10:  [B]j  ^  [Map]j  X  [A]j 

11:  end  if 

12:  for  argmax  ([B]y)  do 

13:  [C]j  ^  [C]j  +  1 

14:  end  for 

15:  end  for 

16:  end  for 

17:  Pj.  <-  argmax  ([C]y  X  [A]j) 

18:  d  ^  Pi-  -  P^> 

19:  m[i]  <—  m[i]  +  5 

20:  end  procedure 


At  this  point,  a  pose  shift  is  calculated  based  upon  the  maximized  pose.  The  maximized  pose 
theta  is  represented  as  the  delta  between  the  angle  from  the  previously  localized  pose  coordinate 
to  the  current  pose  coordinate,  and  the  angle  from  the  same  previous  pose  coordinate  to  the  newly 
maximized  coordinate. 


3.2. 3. 3  Iteration.  At  the  end  of  one  iteration,  we  are  left  with  a  pose  shift  that  is 
added  to  the  accumulated  pose  shift  calculated  in  previous  iterations.  All  future  robot-reported 
pose  readings  over  the  course  of  the  local  map  are  then  adjusted  by  this  accumulated  shift.  Once 
the  distance  of  the  local  map  has  been  traversed,  the  EM  procedure  is  repeated  for  that  map's 
initial  pose  using  our  limited  implementation  of  the  beta  step  described  in  Section  3. 2. 3.1.  A  new 
pose  shift  results,  the  entire  map  is  shifted  as  a  unit,  and  the  accumulated  pose  shift  for  the  global 
map  is  updated.  At  this  point  a  new  local  map  is  initialized  with  a  localized  robot  pose,  the  map  is 
built,  relocalized,  and  the  process  repeats.  This  EM  localization  procedure  is  given  in  Algorithm  2. 
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IV.  Results  and  Analysis 


This  chapter  presents  the  various  results  of  applying  the  mapping  and  localization  techniques 
discussed  in  the  previous  chapter,  along  with  the  many  factors  which  influence  fhem,  such  as 
sensors,  environmenf,  models,  and  paramefers. 

4.1  Mapping 

There  are  a  number  of  paramefers  we  builf  info  fhe  basic  occupancy  grid  mapping  algorifhm 
fhaf  facilifafed  experimenfafion  wifh  factors  fhaf  affected  map  qualify  These  paramefers  are  nof 
localizafion  paramefers,  per  se,  even  fhough  fheir  effecf  on  fhe  resulfing  map  does  indirecfly  affecf 
localizafion.  These  secondary  effecfs  and  ofher  localization-specific  paramefers  are  discussed  in 
section  4.1.2. 

The  mapping-specific  paramefers,  along  wifh  fheir  defaulf  values,  include: 

•  Cell  Size:  fhe  number  of  millimefers  per  grid-world  cell;  one  sonar  range  reading  sweep  is 
processed  per  cell  (100mm) 

•  Sonar  Model:  one  of  Poinf  of  Refurn,  Acoustic  Axis,  and  Field  of  View,  as  infroduced  in  3.1.1 
(Acoustic  Axis) 

•  Ignore  Ouf  of  Range  Readings:  whefher  ouf  of  range  readings  are  processed  or  ignored  (Yes) 

•  Obsfrucfion  value:  fhe  minimum  prior  probabilify  of  occupancy  fhaf  idenfifies  an  obsfrucfion 
along  fhe  acoustic  axis  of  a  sonar  reading  (0.7) 

•  Ignore  Obsfrucfed  Readings:  whefher  to  ignore  an  obsfrucfion,  fhaf  is,  to  process  fhe  reading 
as  if  if  was  unobsfrucfed  (No) 

Addifional  mapping  paramefers  specific  to  fhe  Field  of  View  sonar  model  are  infroduced  in 
section  4. 1.1.1. 
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4.1.1  Sonar  Model.  The  choice  of  sonar  model  came  down  to  which  of  the  three  produced 
maps  most  effective  for  localization.  While  Point  of  Return  produced  visually  appealing  maps 
with  minimal  computation,  it  did  not  provide  sufficient  data.  At  the  opposite  end  of  the  spectrum, 
it  was  our  expectation  that  Field  of  View  would  produce  the  best  maps  in  the  sense  that  they  would 
be  the  richest  in  data,  but  in  actuality,  they  were  the  worst,  especially  in  the  context  of  integrating 
local  maps,  largely  due  to  the  fact  that  a  particular  reading  has  a  large  area  of  influence  that  often 
times  is  rather  destructive,  discussed  in  more  detail  below.  Our  localization  results,  then,  almost 
entirely  relied  on  the  maps  produced  using  the  Acoustic  Axis  model.  Figures  4.1  and  4.2  illustrate 
the  three  models,  with  the  former  processing  all  range  readings  and  the  latter  ignoring  those  that 
are  obstructed  by  previous  readings  as  well  as  those  that  are  out  of  range.  For  these  examples,  an 
obstruction  is  defined  as  a  cell  that  contains  a  prior  probability  of  0.7  or  greater. 

4.1.1.1  Additional  Parameters.  Many  of  the  additional  parameters  are  specific  to  the 
Field  of  View  model,  which  were  inserted  during  development  in  an  effort  to  improve  the  quality 
of  the  map.  They  are  shown  here,  with  their  default  values,  and  illustrated  in  Figures  4.3  through 

4.7. 

•  jS:  The  half  width  of  the  sonar  field  of  view  (15°) 

•  Region  I  Width:  The  size  of  Region  I  (200mm) 

•  Max  F(Occupied),  Region  I:  The  cap  on  the  maximum  probability  of  occupied  for  those 
readings  that  fall  within  Region  I  (0.98) 

•  Max  F(Occupied),  Region  II:  A  cap  on  the  maximum  probability  of  occupied  for  those 
readings  that  fall  within  Region  II  (1.00) 

•  Out  of  Range  Conversion:  Fixed  value  to  which  out  of  range  readings  are  converted  (1000mm) 

•  Disable  Sonars:  Any  sonar  can  be  selectively  enabled  or  disabled,  designed  to  reduce  noise 
(all  enabled) 
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Figure  4.1:  Sonar  model  comparison,  showing  Point  of  Return  (a).  Acoustic  Axis  (b),  and  Field  of 
View  (c)  models.  Obstructed  and  out  of  range  readings  are  not  ignored. 


Figure  4.2:  Sonar  model  comparison,  ignoring  obstructed  and  out  of  range  readings,  showing  Point  of 
Return  (a).  Acoustic  Axis  (b),  and  Field  of  View  (c)  models. 
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The  conclusion  to  be  drawn  from  the  |3  configuration  parameter,  illustrated  in  Figure  4.3,  is 
that  there  is  little  motivation  for  deviating  from  the  sonar  specified  default  of  15°.  This  is  the  value 
that  was  in  fact  used  for  all  testing  in  our  research. 

Region  I  width  was  left  at  the  default  value  for  all  testing,  which  when  mapped  to  the  grid 
world,  resulting  in  a  width  of  1  to  2  cells,  depending  upon  the  heading  of  the  range  reading  vector. 

The  cap  on  the  maximum  probability  of  occupied  for  Region  I,  which  captures  the  notion 
that  there  is  never  100%  certainty  of  occupancy  for  a  given  cell,  was  also  left  at  the  default  value 
of  0.98  for  all  testing. 

Figures  4.4  and  4.5  illustrate  the  effects  of  capping  Region  II  probabilities  in  a  marmer  similar 
to  that  of  Region  I.  Both  map  snapshots  (a)  and  (b)  in  these  figures  show  readings  for  a  sonar  sweep 
in  progress  at  time  t  -  2.  The  large  reading  in  (b)  is  seen  to  have  written  over  the  previous  one 
in  (a).  While  in  this  case  obstructed  readings  are  being  skipped,  this  occurs  because  obstructions 
are  checked  only  along  the  acoustic  axis.  These  same  snapshots  also  specify  the  actual  prior 
probabilities  that  result  from  convolving  the  sonar  reading  with  the  existing  map.  The  boundary 
between  Region  I  and  Region  II  is  readily  discemable,  as  well  as  how  the  two  regions  are  inverses 
of  each  other,  in  accordance  with  the  sonar  model  described  in  section  3.1.1.  For  Region  I,  cells 
closest  to  the  acoustic  axis  receive  higher  probabilities  of  occupancy,  while  for  Region  II,  there  is 
in  effect  a  greater  certainty  of  vacancy. 


Figure  4.3:  Field  of  view  f  configurations,  showing  f  values  of  7°  (a),  15°  (b),  and  30°  (c).  For 
reference,  map  (d)  is  built  using  the  Acoustic  Axis  model. 
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Figure  4.4:  Field  of  view  Region  II  cap  of  0.5,  showing  a  reading  in  (a)  subsequently  overwritten 
by  (b),  resulting  in  final  map  (c). 


Figure  4.5:  Field  of  view  Region  II  cap  of  1.0,  showing  a  reading  in  (a)  subsequently  overwritten 
by  (b),  resulting  in  final  map  (c). 
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The  stark  contrast  between  these  two  figures  illustrates  the  effect  of  capping  fhe  maximum 
probability  in  Region  II  to  0.5  (Figure  4.4)  versus  leaving  it  at  the  default  Murphy  sonar  model 
specification  of  1.0  (Figure  4.5).  If  seems  reasonable  a  cap  of  0.5  would  be  correcf,  since  for  the 
environment  space  laying  between  the  sonar  device  and  the  detected  object,  we  are  representing 
more  the  probability  of  vacancy  fhan  occupancy,  and  so  values  should  fall  befween  0.0  (vacancy 
is  absolufely  cerfain)  fo  0.5  (fhere  is  no  cerfainfy  as  fo  vacancy  or  occupancy).  In  facf,  fhis  is 
how  [32]  implemenfs  fhe  model.  Surprisingly,  however,  leaving  fhe  paramefer  as  specified  by 
Murphy  (seffing  if  to  1.0  in  effect  removes  the  cap)  and  allowing  values  to  fall  befween  0.0  and  1.0 
produces  fhe  beffer  resulf.  The  "burrowing"  effecf  is  much  more  pronounced  in  Figure  4.5(c),  and 
fhe  hallway  indeed  discernable;  in  Figure  4.4(c),  fhe  hallway  is  almosf  complefely  obliferafed. 

When  ouf  of  range  readings  are  processed  for  fhe  Field  of  View  model,  one  of  fwo  things  can 
occur  within  our  implementation:  it  can  be  ignored  entirely,  which  means  it  is  skipped  and  the  next 
reading  is  processed;  or  it  can  be  converted  and  processed  as  an  in-range  reading.  These  options  are 
illustrated  in  Figure  4.6,  where  three  different  conversions  are  shown  along  with  representations 


Figure  4.6:  Range  reading  configurations,  showing  out  of  range  readings  ignored  (a)  or  converfed 
fo  1000mm  (b),  1500mm  (c),  and  2500mm  (d). 
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where  the  out  of  range  readings  are  ignored.  The  maximum  range  of  fhe  sonar  for  fhe  Pioneer 
is  3000mm,  so  fhe  2500mm  opfion  is  shown  less  for  ufilify  and  more  for  ifs  desfrucfive  effecf. 
For  the  upper  map,  the  1500mm  setting  produced  the  cleanest  map,  although  only  marginally 
over  the  1000mm  setting  (as  can  be  seen  by  comparing  the  lower-right  comers).  For  the  lower 
map,  the  1000mm  setting  was  best,  which  is  more  clearly  seen  in  that  it  virtually  eliminated  the 
phantom  island  that  appeared  using  higher  conversion  values.  The  key  difference  befween  fhese 
fwo  environmenfs  is  fhe  upper  map  is  a  hallway  wifh  no  open  areas;  fhe  lower  map  has  a  large 
open  space  fhaf  better  demonsfrafes  fhe  spurious  Field  of  View  readings  fhaf  occur  in  such  a 
configuration.  This  suggests  that  while  1500mm  may  be  best  in  the  absence  of  open  spaces,  fhe 
1000mm  settings  works  almosf  equally  well,  and  has  fhe  advanfage  of  handling  open  spaces  better. 

The  final  paramefer  configuration  wifhin  our  implementation  that  is  most  applicable  to  the 
Field  of  View  model  (alfhough  if  affecfs  fhe  ofhers  as  well),  is  fhaf  of  enabling  and  disabling 
specific  sonars,  illusfrafed  in  Figure  4.7.  For  fhe  maps  shown,  ouf  of  range  readings  are  converfed 


Figure  4.7:  Sonar  configurations,  showing  effecfs  of  enabling  cerfain  sonars,  as  indicafed  by  fhe 
insef  sonar  array  diagrams  (where  shading  indicafes  an  enabled  sonar). 
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to  1000mm,  and  obstructed  readings  are  not  ignored.  This  parameter  highlights  the  noisy  nature 
of  sonars,  especially  when  considering  those  that  strike  a  wall  in  a  non-orthogonal  marmer.  In  this 
environment  and  with  this  model,  however,  the  extra  noise  serves  to  clean  up  path  (a),  largely  as 
a  result  of  fhe  ouf  of  range  reading  conversion;  fhe  ofher  configurations  each  leave  fhe  pafh  wifh 
more  residue,  alfhough  walls  generally  end  up  being  more  well  defined  (as  seen  in  fhe  upper  and 
lower  hallways).  Because  fhe  Field  of  View  model  is  besf  considered  as  a  "burrowing"  mapper, 
fhe  maps  wifh  fhe  cleaner  walls  are  acfually  inferior  fo  (a). 

4.1.2  Localization.  The  goal  of  fhis  research  efforf  was  fo  achieve  a  form  of  local  localizafion, 
fhaf  is,  localizafion  wifhin  a  single  room,  fhaf  would  confribufe  fo  a  higher  level  localizafion 
befween  rooms.  Our  initial  fesfing  was  done  in  fhe  confexf  of  reconciling  pose  shiff  fhaf  occurs 
when  mapping  cycles,  such  as  hallways.  In  several  cases  fhese  resulfs  were  good,  as  demonsfrafed 
in  Figures  4.8  fhrough  4.10.  A  negafive  resulf,  which  is  characferisfic  of  affempfing  fo  localize  in 
large  environmenfs,  is  shown  in  Figure  4.11. 

Figure  4.8  shows  fhe  boffom  half  of  a  corridor  in  fhe  shape  of  a  figure  8  fhaf  spans  roughly 
44m.  The  upper  leff  and  righf  corners  show  openings  fhaf  correspond  fo  fhe  hallways  fhaf  exfend 
upward  in  fhaf  direcfion.  For  fhis  simulafion  run,  fhe  local  map  size  was  5m,  ouf  of  range 
readings  were  skipped,  and  cells  wifh  probabilify  of  occupancy  0.7  or  higher  were  considered  as 
obsfrucfions.  The  improvemenf  in  fhe  map  as  a  resulf  of  our  localizafion  is  mosf  readily  seen  by 
looking  af  fhe  lower  righf  corner  where  fhe  robof  refums  fo  ifs  sfarfing  position  and  closes  fhe 
loop.  In  fhis  case,  you  can  see  fhe  gap  is  smaller. 

The  second  example.  Figure  4.9,  shows  fhe  robof  mapping  fhe  same  environmenf  as  fhaf  in 
Figure  4.8,  buf  in  fhis  case,  fhe  pafh  circles  fhe  enfire  corridor,  rafher  fhan  jusf  fhe  lower  half,  abouf 
66m  in  lengfh.  The  openings  fo  fhe  hallway  fhaf  cormecfs  fhe  easf  and  wesf  corridors  are  visible  on 
fhe  inferior  wall  in  fhe  middle  of  fhe  map.  The  local  map  size  was  5m,  ouf  of  range  readings  were 
skipped,  and  cells  wifh  probabilify  of  occupancy  0.7  or  higher  were  considered  as  obsfrucfions. 


55 


Figure  4.8:  Localization  result  A 


Figure  4.9:  Localization  result  B 
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Again,  the  correction  resulting  from  localization  is  apparent  when  focusing  on  fhe  closure  of  fhe 
cycle,  where  fhe  misalignmenf  in  fhe  original  map  (a)  is  complefely  aligned  in  (b). 


Figure  4.10  shows  a  real-world  example  of  improved  localizafion.  In  fhis  fesf,  fhe  robof 
sfarf  posifion  was  af  fhe  upper  righf-hand  corner,  and  ifs  pafh  exfended  forward  a  shorf  disfance, 
leff  around  a  90°  furn,  down  a  hallway  wifh  doorways  and  obsfacles  along  fhe  wall,  and  affer 
a  180°  fum,  back  fo  fhe  sfarf  posifion,  for  a  fofal  disfance  fraveled  of  approximafely  50m.  The 
seffings  were  fhe  same  as  for  fhe  fwo  runs  jusf  discussed.  The  original  map  (a)  shows  plainly  fhe 
odomefry  error  fhaf  occurred  affer  making  fhe  180°  fum.  In  fhe  localized  map  (b),  fhere  is  some 
correction,  buf  fhe  pose  of  fhe  local  map  creafed  as  fhe  robof  approached  fhe  90°  righf  furn  back 
fo  ifs  sfarfing  posifion  was  nof  correcfed  sufficienfly.  This  is  because  fhe  driff  was  greaf  enough 
fhaf  fhe  original  wall  ended  up  in  fhe  cenfer  of  fhe  new  pafh  and  fhe  localizafion  procedure  was 
unable  fo  compensafe. 
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Figure  4.10: 
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Localization  result  C 
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(a)  (b) 

Figure  4.11:  Localization  result  D 


The  correspondence  problem  is  magnified  in  the  large  cycle  of  Figure  4.11,  which  covers 
approximately  120m  of  distance.  For  this  real-world  run,  the  robot's  starting  position  is  seen  at  the 
right  side  of  the  figure.  It's  path  extends  up  toward  the  top  of  the  figure,  makes  a  90°  turn  down 
toward  the  left  side,  and  after  two  more  turns,  makes  its  way  back  to  the  starting  position  (which 
it  sorely  misses),  and  then  revisits  the  initial  corridor,  and  stops  after  one  final  turn  (shown  in  the 
upper  left-hand  comer  of  the  figure).  This  example  highlights  the  problem  that  occurs  when  the 
robot  does  not  soon  return  to  a  portion  of  the  environment  is  has  previously  visited.  Another  factor 
is  the  extent  of  the  odometry  error,  which  becomes  significant  at  the  second  turn  (shown  at  the 
lower  left-hand  comer  of  the  figure).  Our  localization  algorithm  is  unable  to  make  any  correction 
to  this  map. 

Our  final  example  (Figure  4.12)  illustrates  an  environment  that  is  in  contrast  to  the  last,  in 
that  it  is  a  room  that  spans  a  much  shorter  distance  (being  approximately  14m  long  by  6m  wide).  In 
this  case,  we  ignored  obstructions,  and  the  local  map  size  was  set  to  3m.  The  robot  path  started  in 
the  doorway  shown  in  the  lower  left-hand  comer  of  (a),  made  a  circle  around  and  loop  within  the 
room,  exited  through  the  doorway  (b),  traveled  the  adjacent  hallway,  reentered  the  room  through 
a  second  doorway,  and  made  a  final  trip  around  the  room  back  to  its  starting  point.  Figures  (a) 
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Figure  4.12:  Localization  result  E 


and  (b)  show  the  single  run  in  two  halves.  Figure  (c)  shows  the  resulting  unlocalized  map,  and 
(d)  our  attempt  at  localization.  The  first  leg  of  fhe  mapping  cycle  (a)  shows  very  little  odomefry 
error,  even  wifh  a  large  number  of  fums  and  a  loop  around  an  obsfacle  in  fhe  room's  confer.  If 
is  nof  until  leaving  the  room  and  making  a  sharp  90°  turn  to  the  left  that  any  noticeable  error 
occurs.  Again,  it's  significant  enough  that  our  procedure  is  unable  to  correct  for  if,  and  once  fhe 
run  is  complefe,  we  are  left  wifh  a  map  nof  much  differenf  from  the  original,  with  the  exception  of 
significanf  addifional  noise. 
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It  is  our  belief  that  one  contributing  factor  to  our  poor  results  was  that  the  pursuit  of  real-time 
localization  limited  our  implementation  of  the  alpha-beta  algorithm  (Section  3.2.3)  to  such  a  degree 
as  to  not  be  consistently  reliable.  Rather  than  relocalize  all  local  maps  at  each  iteration,  we  localize 
the  current  map  at  two  points:  at  its  creation  (alpha),  and  at  its  completion  (beta),  both  from  its 
point  of  origin. 

Another  factor  is  the  local  aspect  of  our  localization.  Localization  only  occurs  at  one  point  for 
each  local  map  (at  two  different  times,  as  just  discussed).  The  pose  distribution,  that  is,  the  region 
considered  for  possible  pose  correction,  only  composes  a  small  section  of  the  world.  As  a  result, 
when  the  robot  is  traveling  over  large  areas,  our  implementation  has  no  knowledge  of  large  scale 
drift,  as  evidenced  by  Figure  4.11.  For  smaller  environments,  like  the  one  shown  in  Figure  4.12, 
the  prior  knowledge  is  confined.  Unfortunately  the  improvement  seen  in  this  particular  example 
is  minimal. 

Our  conclusion  is  the  implementation  shows  promise,  but  needs  to  be  extended  and  refined, 
as  discussed  in  Chapter  5. 


60 


V.  Future  Work  and  Conclusions 


This  chapter  discusses  a  number  of  extensions  that  we  feel  would  result  in  improved  localization 
results,  and  then  presents  concluding  remarks  regarding  our  research  effort. 

5.1  Future  Extensions 

One  of  our  localization  steps  may  be  contributing  to  the  noise  of  our  resulting  maps.  Specif¬ 
ically,  knowledge  of  the  environment  is  only  partial  for  the  localization  step  that  occurs  at  the 
creation  of  a  local  map.  That  is,  only  the  environment  to  the  rear  has  been  explored,  and  that  to 
the  front  remains  unexplored,  with  the  exception  of  those  instances  when  the  robot  is  revisiting 
a  previously  explored  area  of  the  environment.  Additionally,  the  localization  is  occurring  using 
sonar  data  acquired  at  a  position  that  is  up  to  100mm  further  along  the  path  than  that  at  which  the 
data  it  is  localizing  against  was  acquired.  An  alternative  to  this  new  map  localization  is  to  slide 
our  two  localization  steps  back  one  time  cycle;  that  is,  localize  the  maps  at  t  -  2  and  t  -  1  rather 
than  t  and  t  -  1.  This  will  provide  the  localization  algorithm  with  more  complete  environment 
data  from  which  it  can  better  maximize  the  expected  pose. 

As  concerns  localization  of  the  map  as  a  whole,  it  would  be  worthwhile  to  expand  the  beta 
step  such  that  it  traverses  further  back  in  time  than  the  one  step  we  presently  have.  Ideally,  a  full 
traversal  back  to  t  =  0  would  be  implemented,  but  the  real-time  constraints  prohibit  this.  It  is 
reasonable  that  the  extent  of  the  beta  calculation  could  be  d5mamic  in  the  sense  that  as  time  allows, 
the  calculation  continues.  This  would  accommodate  the  d5mamic  nature  of  the  environment, 
where  some  localization  steps  take  less  time  to  process  than  others  by  virtue  of  limited  sonar  data 
or  a  smaller  local  map  size. 

Our  expectation  calculation  uses  independent  sonar  voting  (Section  3.2. 3. 2),  meaning,  each 
sonar  has  a  vote  on  which  robot  pose  within  the  pose  distribution  best  fits  its  reading  given  the 
map,  and  each  sonar  is  considered  independently  of  the  others.  In  other  words,  each  sonar  updates 


61 


the  pose  histogram  without  consideration  for  any  of  fhe  ofher  readings.  A  more  robusf  approach 
would  be  fo  combine  fhe  vofes  of  all  sonars  af  each  poinf  in  fhe  pose  disfribufion  so  fhaf  fhe  pose 
fhaf  simulfaneously  besf  fifs  all  fhe  readings  would  be  selecfed.  This  would  nof  cosf  any  more 
compufafionally  because  fhe  same  number  of  calculafions  would  be  performed,  and  would  lead 
fo  a  beffer-informed  expecfafion  calculafion. 


Figure  5.1:  Map  segment  with  unrecoverable  pose  drift 


In  cerfain  environmenfs,  especially  fhose  where  fhe  robof  is  visiting  areas  if  has  previously 
been,  if  may  be  worfhwhile  fo  localize  using  fhe  global  map  as  if  exisfed  prior  fo  fhe  consfrucfion 
of  fhe  currenf  local  map.  For  example.  Figure  5.1  shows  an  insfance  where  doing  so  may  correcf 
for  fhe  pose  driff  fhaf  occurred  when  fhe  robof  reached  fhe  end  of  fhe  hall,  fumed  approximafely 
180°,  and  proceeded  back  down  fhe  same  pafh.  Consider  fhe  refum  pafh  shown  fo  be  mapped 
over  fwo  local  maps.  If  af  fhe  end  of  fhe  firsf  fhe  robof 's  reporfed  pose  is  compared  wifh  fhe  global 
map  before  fhaf  map  is  convolved,  if  would  have  a  greafer  likelihood  of  defermining  if  is  on  fhe 
wrong  side  of  fhe  wall  and  correcf  for  if.  In  our  implemenfafion,  fhe  local  map  is  convolved  firsf, 
when  in  a  scenario  as  fhis  wifh  such  a  large  driff,  fhe  abilify  fo  recover  quickly  becomes  impossible. 

Two  additional  exfensions  involve  fhe  collecfion  of  sonar  range  dafa.  The  firsf  is  fo  change 
fhe  frequency  wifh  which  sonar  sweeps  are  processed  for  a  given  cell.  In  our  implemenfafion,  we 
process  one  sweep.  While  processing  mulfiple  sweeps  while  fhe  robof  is  sfaf ionary  would  likely 
yield  liffle  change,  doing  so  while  if  moves  over  fhe  course  of  a  single  cell  may  see  a  reduction  in 
noise  fhaf  would  be  beneficial  fo  cleaning  fhe  maps.  Anofher  means  of  cleaning  fhe  sonar  dafa 
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would  be  to  take  those  readings  that  can  be  identified  as  spurious  when  compared  to  adjacent  and 
prior  readings  and  modify  fhem  such  fhaf  fhey  sfrengfhen  fhose  ofher  readings. 

Anofher  facef  of  fhis  involves  fhe  facf  fhaf  in  our  implemenfafion,  if  fhe  robof's  position  is 
nof  changing,  sonar  dafa  is  nof  collecfed,  even  if  if  is  fuming.  As  a  means  of  acquiring  greafer 
knowledge,  if  may  be  worfhwhile  fo  nof  only  incorporafe  sonar  dafa  as  fhe  robof  moves,  buf  also 
as  if  fums,  which  would  be  mosf  beneficial  around  comers  where  fhe  robof  is  mosf  likely  fo  slow 
down  and  possibly  furn  several  if  nof  many  degrees  over  fhe  course  of  100mm. 

5.2  Conclusion 

The  objecfive  of  fhis  research  efforf  was  fo  explore  and  develop  a  real-time  sonar-based  robof 
mapping  and  localizafion  algorifhm  fhaf  provides  pose  correction  wifhin  fhe  confexf  of  a  single 
room,  fo  be  combined  wifh  pre-exisfing  global  localizafion  fechniques,  and  fhus  produce  a  single, 
well-formed  map  of  an  unknown  environmenf.  The  parficular  direction  chosen  was  based  on  fhe 
work  of  [10,43,45],  specifically  in  fhe  confexf  of  Baum- Welch-based  expecfed  pose  maximization. 
Our  algorifhm  implemenfed  a  reduced  form  of  fhe  alpha-befa  algorifhm,  performing  fhe  forward 
alpha  calculation  as  an  infegral  componenf  of  fhe  occupancy  grid  mapping  procedure  using  local 
maps,  and  a  backward  befa  calculation  fhaf  considered  fhe  map  af  f  -  1  only. 

Real-time  localizafion  is  an  exfremely  difficulf  fask  fhaf  continues  fo  be  fhe  focus  of  much 
research  in  fhe  field  of  aufonomous  robotic  mapping.  Mosf  advances  in  localizafion  have  been 
achieved  in  an  off-line  confexf.  The  resulfs  of  our  research  and  implemenfafion  did  nof  produce 
fhe  desired  localizafion  in  a  consisfenf  number  of  cases,  a  facfor  we  believe  is  mosf  direcfly 
affribufable  fo  fhe  limifed  befa  calculafion  and  fhe  limifed  knowledge  utilized  by  fhe  algorifhm  af 
each  localizafion  sfep.  Unforfunafely,  fhe  processing  requiremenfs  fo  perform  a  full  befa  calculafion 
carmof  be  accomplished  in  real  time  wifh  currenf  compufer  resources.  However,  we  believe  fhere 
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is  ample  room  for  extension  of  this  particular  research,  as  outlined  above,  that  may  well  lead  to  a 
successful  real-time  local  localization  algorithm. 
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